/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Turn off the RGB LED.
*/
palSetLine(LINE_LED_RED); /* red */
palSetLine(LINE_LED_GREEN); /* green */
palSetLine(LINE_LED_BLUE); /* blue */
/*
* Create the button check thread.
*/
chThdCreateStatic(waButtonThread, sizeof(waButtonThread), NORMALPRIO, ButtonThread, NULL);
/*
* Start the PWM driver, route TPM0 output to PTE29, PTE31, PTD5.
* Enable channels now to avoid a blink later.
*/
pwmStart(&PWM_DRIVER, &pwmcfg);
palSetLineMode(LINE_LED_RED, PAL_MODE_ALTERNATIVE_3);
palSetLineMode(LINE_LED_GREEN, PAL_MODE_ALTERNATIVE_3);
palSetLineMode(LINE_LED_BLUE, PAL_MODE_ALTERNATIVE_4);
pwmEnableChannel(&PWM_DRIVER, 2, 0);
pwmEnableChannel(&PWM_DRIVER, 4, 0);
pwmEnableChannel(&PWM_DRIVER, 5, 0);
/*
* Create the breathe thread.
*/
chThdCreateStatic(waBreatheThread, sizeof(waBreatheThread), NORMALPRIO, BreatheThread, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state, when the button is
* pressed ... nothing happens.
*/
while(true) {
chThdSleepMilliseconds(500);
}
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
palClearLine(LINE_LED_GREEN);
chThdSleepMilliseconds(50);
palClearLine(LINE_LED_RED);
chThdSleepMilliseconds(200);
palSetLine(LINE_LED_GREEN);
chThdSleepMilliseconds(50);
palSetLine(LINE_LED_RED);
chThdSleepMilliseconds(200);
}
}
static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argv;
if (argc != 1) {
chprintf(chp, "Usage: bias [sample|reset]\r\n");
return;
}
if(!strcmp (argv[0], "sample")) {
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
chprintf(chp, "Please don't move the device while Green LED is on!\r\n");
chprintf(chp, "Press a key to start...\r\n");
while (chnGetTimeout((BaseChannel *)chp, 500) == Q_TIMEOUT)
;
palSetLine(LINE_LED4);
chThdSleepMilliseconds(1000);
gyroscopeSampleBias(&L3GD20D1);
palClearLine(LINE_LED4);
chprintf(chp, "Procedure completed!\r\n");
}
else if(!strcmp (argv[0], "reset")) {
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
gyroscopeResetBias(&L3GD20D1);
chprintf(chp, "Bias correction removed!\r\n");
}
else {
chprintf(chp, "Usage: bias [sample|reset]\r\n");
return;
}
}
static void sp100_check_status(int n) {
/* TODO Signal the error over serial too */
if(sp100_status(n) & (SP100_STATUS_PARITY |
SP100_STATUS_CHECKSUM |
SP100_STATUS_LOWV |
SP100_STATUS_FAULT))
{
palSetLine(LINE_LED_RED);
}
}
static void blinker(void) {
OS_TaskRegister();
while (true) {
palSetLine(LINE_ARD_D13);
OS_TaskDelay(500);
palClearLine(LINE_ARD_D13);
OS_TaskDelay(500);
}
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
systime_t time;
time = serusbcfg.usbp->state == USB_ACTIVE ? 250 : 500;
#if defined(BOARD_ST_STM32F4_DISCOVERY)
palClearLine(LINE_LED4);
chThdSleepMilliseconds(time);
palSetLine(LINE_LED4);
chThdSleepMilliseconds(time);
#else
palClearLine(LINE_LED);
chThdSleepMilliseconds(time);
palSetLine(LINE_LED);
chThdSleepMilliseconds(time);
#endif
}
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
systime_t time = serusbcfg1.usbp->state == USB_ACTIVE ? 250 : 500;
palClearLine(LINE_LED);
chThdSleepMilliseconds(time);
palSetLine(LINE_LED);
chThdSleepMilliseconds(time);
}
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
palSetLineMode(LINE_LED_GREEN, PAL_MODE_OUTPUT_PUSHPULL);
while (true) {
palClearLine(LINE_LED_GREEN);
chThdSleepMilliseconds(500);
palSetLine(LINE_LED_GREEN);
chThdSleepMilliseconds(500);
}
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
systime_t time;
time = USBD2.state == USB_ACTIVE ? 250 : 500;
palClearLine(LINE_ARD_D13);
chThdSleepMilliseconds(time);
palSetLine(LINE_ARD_D13);
chThdSleepMilliseconds(time);
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Route USART3 to PC4/TX/pin1 PC5/RX/pin0
*/
palSetLineMode(LINE_PIN0, PAL_MODE_ALTERNATE(7));
palSetLineMode(LINE_PIN1, PAL_MODE_ALTERNATE(7));
/*
* Activates the serial driver 2 using the driver default configuration.
*/
sdStart(&SD3, NULL);
// palSetLineMode(LINE_LED_GREEN, PAL_MODE_OUTPUT_PUSHPULL);
palClearLine(LINE_LED_GREEN);
chThdSleepMilliseconds(500);
palSetLine(LINE_LED_GREEN);
// palSetPadMode(GPIOC, 10, PAL_MODE_OUTPUT_PUSHPULL);
// palTogglePad(GPIOC, 10);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (true) {
if (palReadLine(LINE_BUTTON)) {
palToggleLine(LINE_LED_BLUE);
// test_execute((BaseSequentialStream *)&SD3);
}
chThdSleepMilliseconds(500);
}
}
static void sp100_wait_measurement(int n) {
/* TODO Signal the error over serial too */
systime_t start_time = chVTGetSystemTime();
while(sp100_status(n) & SP100_STATUS_MEASURE) {
/* If we've been waiting more than 50ms, signal an error and
* continue anyway.
*/
if(ST2MS(chVTGetSystemTime() - start_time) > 50) {
palSetLine(LINE_LED_RED);
return;
}
}
}
bool AP_IOMCU_FW::handle_code_write()
{
switch (rx_io_packet.page) {
case PAGE_SETUP:
switch (rx_io_packet.offset) {
case PAGE_REG_SETUP_ARMING:
reg_setup.arming = rx_io_packet.regs[0];
break;
case PAGE_REG_SETUP_FORCE_SAFETY_OFF:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_off();
reg_status.flag_safety_off = true;
} else {
return false;
}
break;
case PAGE_REG_SETUP_FORCE_SAFETY_ON:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_on();
reg_status.flag_safety_off = false;
} else {
return false;
}
break;
case PAGE_REG_SETUP_ALTRATE:
reg_setup.pwm_altrate = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_PWM_RATE_MASK:
reg_setup.pwm_rates = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_DEFAULTRATE:
if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 25;
}
if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 400;
}
reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
update_default_rate = true;
break;
case PAGE_REG_SETUP_SBUS_RATE:
reg_setup.sbus_rate = rx_io_packet.regs[0];
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
break;
case PAGE_REG_SETUP_FEATURES:
reg_setup.features = rx_io_packet.regs[0];
/* disable the conflicting options with SBUS 1 */
if (reg_setup.features & (P_SETUP_FEATURES_SBUS1_OUT)) {
reg_setup.features &= ~(P_SETUP_FEATURES_PWM_RSSI |
P_SETUP_FEATURES_ADC_RSSI |
P_SETUP_FEATURES_SBUS2_OUT);
// enable SBUS output at specified rate
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
} else {
palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
}
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = AP_HAL::millis();
break;
case PAGE_REG_SETUP_REBOOT_BL:
if (reg_status.flag_safety_off) {
// don't allow reboot while armed
return false;
}
// check the magic value
if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) {
return false;
}
schedule_reboot(100);
break;
default:
break;
}
break;
case PAGE_DIRECT_PWM: {
/* copy channel data */
uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i];
}
offset++;
num_values--;
i++;
}
fmu_data_received_time = AP_HAL::millis();
reg_status.flag_fmu_ok = true;
reg_status.flag_raw_pwm = true;
chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
break;
}
default:
break;
}
memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
tx_io_packet.count = 0;
tx_io_packet.code = CODE_SUCCESS;
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
return true;
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL);
/*
* L3GD20 Object Initialization
*/
l3gd20ObjectInit(&L3GD20D1);
/*
* Activates the L3GD20 driver.
*/
l3gd20Start(&L3GD20D1, &l3gd20cfg);
while(!palReadLine(LINE_BUTTON)){
chprintf(chp, "Press BTN to calibrate gyroscope...\r\n");
chThdSleepMilliseconds(150);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
}
chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n");
chprintf(chp, "Keep it in the rest position while red LED is on\r\n");
chThdSleepMilliseconds(3000);
palSetLine(LINE_LED10_RED);
chThdSleepMilliseconds(1000);
gyroscopeSampleBias(&L3GD20D1);
palClearLine(LINE_LED10_RED);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
while (TRUE) {
palToggleLine(LINE_LED10_RED);
gyroscopeReadRaw(&L3GD20D1, rawdata);
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
chprintf(chp, "RAW-%c:%d\r\n", axesID[i], rawdata[i]);
gyroscopeReadCooked(&L3GD20D1, cookeddata);
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]);
gyroscopeGetTemp(&L3GD20D1, &temperature);
chprintf(chp, "TEMP:%.1f C°\r\n", temperature);
chThdSleepMilliseconds(150);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
}
l3gd20Stop(&L3GD20D1);
}
