static msg_t LinkInThread(void *arg) {
chRegSetThreadName("MAVLinkIn");
SerialDriver *sdp = (SerialDriver *)arg;
UnpackCycle(sdp);
chThdExit(0);
return 0;
}
int main(void)
{
halInit();
chSysInit();
rccEnableAHB2(RCC_AHB2ENR_RNGEN, 0);
{
LedCfg cfg = {GPIOD, GPIOD_LED_ORANGE};
orangeTP = chThdCreateStatic(orangeStack, sizeof(orangeStack), THD_PRIO, ledThd, &cfg);
}
{
LedCfg cfg = {GPIOD, GPIOD_LED_RED};
redTP = chThdCreateStatic(redStack, sizeof(redStack), THD_PRIO, ledThd, &cfg);
}
{
LedCfg cfg = {GPIOD, GPIOD_LED_BLUE};
blueTP = chThdCreateStatic(blueStack, sizeof(blueStack), THD_PRIO, ledThd, &cfg);
}
{
LedCfg cfg = {GPIOD, GPIOD_LED_GREEN};
greenTP = chThdCreateStatic(greenStack, sizeof(greenStack), THD_PRIO, ledThd, &cfg);
}
chThdExit(1);
return 0;
}
/**
* Command processing thread.
*/
msg_t CmdExecutor::main(void){
this->setName("CmdExecutor");
eventmask_t evt = 0;
struct EventListener el_command_long;
chEvtRegisterMask(&event_mavlink_in_command_long, &el_command_long, EVMSK_MAVLINK_IN_COMMAND_LONG);
/* wait modems */
while(GlobalFlags.messaging_ready == 0)
chThdSleepMilliseconds(50);
/* main loop */
while(!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_COMMAND_LONG, MS2ST(50));
switch (evt){
case EVMSK_MAVLINK_IN_COMMAND_LONG:
executeCmd(&mavlink_in_command_long_struct);
break;
default:
break;
}
}
chEvtUnregister(&event_mavlink_in_command_long, &el_command_long);
chThdExit(0);
return 0;
}
void osDeleteTask(OsTask *task)
{
//Delete the specified task
if(task == NULL)
chThdExit(RDY_OK);
else
chThdTerminate(task->tp);
}
/**
* HTTP server thread.
*/
THD_FUNCTION(http_server, p) {
struct netconn *conn, *newconn;
err_t err;
(void)p;
chRegSetThreadName("http");
/* Create a new TCP connection handle */
conn = netconn_new(NETCONN_TCP);
LWIP_ERROR("http_server: invalid conn", (conn != NULL), chThdExit(MSG_RESET););
/**
* @brief Thread termination.
* @note The thread is not really terminated but asked to terminate which
* is not compliant.
*/
osStatus osThreadTerminate(osThreadId thread_id) {
if (thread_id == osThreadGetId()) {
/* Note, no memory will be recovered unless a cleaner thread is
implemented using the registry.*/
chThdExit(0);
}
chThdTerminate(thread_id);
chThdWait((thread_t *)thread_id);
return osOK;
}
/**
*
* @brief Destroys an instance of @p struct pios_thread.
*
* @param[in] threadp pointer to instance of @p struct pios_thread
*
*/
void PIOS_Thread_Delete(struct pios_thread *threadp)
{
if (threadp == NULL)
{
chThdExit(0);
}
else
{
chThdTerminate(threadp->threadp);
chThdWait(threadp->threadp);
}
}
/*
* PPRZ/AP thread
*
* Call PPRZ AP periodic and event functions
*/
static void thd_ap(void *arg)
{
(void) arg;
chRegSetThreadName("AP");
while (!chThdShouldTerminateX()) {
Ap(handle_periodic_tasks);
Ap(event_task);
chThdSleepMicroseconds(500);
}
chThdExit(0);
}
static msg_t batterySurveyThd(void *arg)
{
(void)arg;
chRegSetThreadName ("battery survey");
chEvtRegister(&powerOutageSource, &powerOutageListener, 1);
chThdSleepMilliseconds (2000);
register_adc_watchdog((uint32_t) ADC1, 4,
V_ALERT, 0xfff, &powerOutageIsr);
chEvtWaitOne(EVENT_MASK(1));
chibios_logFinish ();
chThdExit(0);
return 0;
}
static THD_FUNCTION(BlueBlinkThread, arg) {
chRegSetThreadName("NormalBlink");
(void)arg;
const int16_t *rx;
msg_t status = MSG_RESET;
while(!chThdShouldTerminateX()){
status = blue_blink_mb.fetch(&rx, MS2ST(100));
if (MSG_OK == status){
blink_cycle(rx, BLINKER_NORMAL);
}
}
chThdExit(MSG_OK);
}
static THD_FUNCTION(RedBlinkThread, arg) {
chRegSetThreadName("WarningBlink");
(void)arg;
const int16_t *rx;
msg_t status = MSG_RESET;
while(!chThdShouldTerminateX()){
status = red_blink_mb.fetch(&rx, MS2ST(100));
if (MSG_OK == status){
blink_cycle(rx, BLINKER_WARNING);
}
}
chThdExit(MSG_OK);
}
static msg_t PollAccelThread(void *semp){
chRegSetThreadName("PollAccel");
msg_t sem_status = RDY_OK;
struct EventListener self_el;
chEvtRegister(&init_event, &self_el, INIT_FAKE_EVID);
while (TRUE) {
sem_status = chBSemWaitTimeout((BinarySemaphore*)semp, MS2ST(20));
txbuf[0] = ACCEL_STATUS;
if ((i2c_transmit(mma8451addr, txbuf, 1, rxbuf, 7) == RDY_OK) && (sem_status == RDY_OK)){
raw_data.xacc = complement2signed(rxbuf[1], rxbuf[2]);
raw_data.yacc = complement2signed(rxbuf[3], rxbuf[4]);
raw_data.zacc = complement2signed(rxbuf[5], rxbuf[6]);
/* there is no need of correcting of placement. Just get milli g */
mavlink_raw_imu_struct.xacc = raw_data.xacc * *xpol;
mavlink_raw_imu_struct.yacc = raw_data.yacc * *ypol;
mavlink_raw_imu_struct.zacc = raw_data.zacc * *zpol;
comp_data.xacc = 1000 * (((int32_t)raw_data.xacc) * *xpol + *xoffset) / *xsens;
comp_data.yacc = 1000 * (((int32_t)raw_data.yacc) * *ypol + *yoffset) / *ysens;
comp_data.zacc = 1000 * (((int32_t)raw_data.zacc) * *zpol + *zoffset) / *zsens;
/* fill scaled debug struct */
mavlink_scaled_imu_struct.xacc = comp_data.xacc;
mavlink_scaled_imu_struct.yacc = comp_data.yacc;
mavlink_scaled_imu_struct.zacc = comp_data.zacc;
}
else{
raw_data.xacc = -32768;
raw_data.yacc = -32768;
raw_data.zacc = -32768;
mavlink_raw_imu_struct.xacc = -32768;
mavlink_raw_imu_struct.yacc = -32768;
mavlink_raw_imu_struct.zacc = -32768;
mavlink_scaled_imu_struct.xacc = -32768;
mavlink_scaled_imu_struct.yacc = -32768;
mavlink_scaled_imu_struct.zacc = -32768;
}
if (chThdSelf()->p_epending & EVENT_MASK(SIGHALT_EVID))
chThdExit(RDY_OK);
}
return 0;
}
static THD_FUNCTION(PulseThread, arg) {
chRegSetThreadName("Pulse");
pulse_t *pulse = arg;
systime_t t = chVTGetSystemTimeX();
while (!chThdShouldTerminateX()) {
t += pulse->high;
palSetPad(GPIOD, pulse->pad);
chThdSleepUntil(t);
palClearPad(GPIOD, pulse->pad);
t+= pulse->low;
chThdSleepUntil(t);
}
chThdExit(MSG_OK);
}
static THD_FUNCTION(can_reader_thread, arg)
{
t_hydra_console *con;
con = arg;
chRegSetThreadName("CAN reader");
chThdSleepMilliseconds(10);
can_rx_frame rx_msg;
mode_config_proto_t* proto = &con->mode->proto;
while (!chThdShouldTerminateX()) {
if(bsp_can_rxne(proto->dev_num)) {
chSysLock();
bsp_can_read(proto->dev_num, &rx_msg);
can_slcan_out(con, &rx_msg);
chSysUnlock();
} else {
chThdYield();
}
}
chThdExit((msg_t)1);
}
static msg_t CanTxThread(void * arg) {
(void)arg;
chRegSetThreadName("CanTx");
CANTxFrame txmsg;
uint32_t i;
txmsg.IDE = CAN_IDE_EXT;
txmsg.EID = 0x01234567;
txmsg.RTR = CAN_RTR_DATA;
txmsg.DLC = 8;
for (i=0; i<8; i++)
txmsg.data8[i] = 0;
// txmsg.data32[0] = 0x55AA55AA;
// txmsg.data32[1] = 0x00FF00FF;
while (!chThdShouldTerminate()) {
i = mpiovd_sensors_struct.analog01;
putinrange(i, 0, 255);
txmsg.data8[0] = i;
i = mpiovd_sensors_struct.analog02;
putinrange(i, 0, 255);
txmsg.data8[1] = i;
i = mpiovd_sensors_struct.analog03;
putinrange(i, 0, 255);
txmsg.data8[2] = i;
i = mpiovd_sensors_struct.analog04;
putinrange(i, 0, 255);
txmsg.data8[3] = i;
/* (>> 1) to exclude "dead" bit from SPI*/
txmsg.data8[4] = (raw_data.discrete >> 1) & 0xFF;
canTransmit(&CAND1, &txmsg, MS2ST(100));
chThdSleepMilliseconds(250);
}
chThdExit(0);
return 0;
}
static THD_FUNCTION(BootBlinkThread, arg) {
chRegSetThreadName("BootBlink");
(void)arg;
while (!chThdShouldTerminateX()) {
red_led_on();
warning_led_off();
PAUSE();
red_led_off();
warning_led_on();
PAUSE();
warning_led_off();
red_led_off();
PAUSE();
}
warning_led_off();
red_led_off();
chThdExit(MSG_OK);
}
static msg_t ShellThread(void *arg){
chRegSetThreadName("Shell");
chThdSleepMilliseconds(1000);
/* init static pointer for serial driver with received pointer */
shell_sdp = (SerialUSBDriver *)arg;
// create and init microrl object
microrl_t microrl_shell;
// cli_print("@@*** Super cool device, version 1.2.3, for help type help... ***@@\r\n");
microrl_init(µrl_shell, cli_print);
// set callback for execute
microrl_set_execute_callback(µrl_shell, execute);
// set callback for completion (optionally)
microrl_set_complete_callback(µrl_shell, complete);
// set callback for ctrl+c handling (optionally)
microrl_set_sigint_callback(µrl_shell, sigint);
while (TRUE){
// put received char from stdin to microrl lib
msg_t c = sdGetTimeout(shell_sdp, MS2ST(50));
if (c != Q_TIMEOUT)
microrl_insert_char(µrl_shell, (char)c);
/* умираем по всем правилам, не забываем убить потомков */
if (chThdShouldTerminate()){
if ((current_cmd_tp != NULL) && (current_cmd_tp->p_state != THD_STATE_FINAL)){
chThdTerminate(current_cmd_tp);
chThdWait(current_cmd_tp);
}
chThdExit(0);
}
}
return 0;
}
THD_FUNCTION(TimeKeeper::TimekeeperThread, arg) {
chRegSetThreadName("Timekeeper");
TimeKeeper *self = static_cast<TimeKeeper *>(arg);
self->GNSS.subscribe(&gps);
while (!chThdShouldTerminateX()) {
if ((gps.fresh) && (gps.fix > 0) && (0 == gps.msec)) {
int64_t tmp = 1000000;
tmp *= mktime(&gps.time);
osalSysLock();
time_gps_us = tmp;
if (! time_verified) {
time_verified = true;
unix_usec = time_gps_us;
}
osalSysUnlock();
/* now correct time in internal RTC (if needed) */
int32_t t1 = time_gps_us / 1000000;
int32_t t2 = rtc_get_time_unix(nullptr);
int32_t dt = t1 - t2;
if (abs(dt) > TIME_CORRECTION_THRESHOLD)
rtc_set_time(&gps.time);
}
if (gps.fresh) {
gps.fresh = false;
}
osalThreadSleepMilliseconds(20);
}
self->GNSS.unsubscribe(&gps);
chThdExit(MSG_OK);
}
static msg_t MmcWriterThread(void *arg){
chRegSetThreadName("MmcWriter");
(void)arg;
struct EventListener el_gps_raw_int;
chEvtRegisterMask(&event_mavlink_gps_raw_int, &el_gps_raw_int, EVMSK_MAVLINK_GPS_RAW_INT);
eventmask_t evt = 0;
/* wait until card not ready */
NOT_READY:
while (!mmcIsCardInserted(&MMCD1))
chThdSleep(SDC_POLLING_INTERVAL);
chThdSleep(SDC_POLLING_INTERVAL);
if (!mmcIsCardInserted(&MMCD1))
goto NOT_READY;
else
_insert_handler();
/* main work cycle */
while (!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_GPS_RAW_INT, WRITE_TMO);
if (!mmcIsCardInserted(&MMCD1)){
_remove_handler();
goto NOT_READY;
}
else{
bnapStorageAcquire(&Storage);
bnapStorageDoRecord(&Storage);
bnapStorageRelease(&Storage);
}
}
chEvtUnregister(&event_mavlink_gps_raw_int, &el_gps_raw_int);
chThdExit(0);
(void)evt;
return 0;
}
static msg_t PlannerThread(void* arg){
chRegSetThreadName("Planner");
(void)arg;
while(GlobalFlags.messaging_ready == 0)
chThdSleepMilliseconds(50);
struct EventListener el_mission_request_list;
struct EventListener el_mission_count;
struct EventListener el_mission_clear_all;
struct EventListener el_mission_item;
chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
chEvtRegisterMask(&event_mavlink_in_mission_count, &el_mission_count, EVMSK_MAVLINK_IN_MISSION_COUNT);
chEvtRegisterMask(&event_mavlink_in_mission_clear_all, &el_mission_clear_all, EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL);
chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
eventmask_t evt = 0;
while (!chThdShouldTerminate()) {
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST |
EVMSK_MAVLINK_IN_MISSION_COUNT |
EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL |
EVMSK_MAVLINK_IN_MISSION_ITEM,
MS2ST(100));
switch (evt){
/* ground want to know how many items we have */
case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST:
chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
mav2gcs();
chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
break;
/* ground says how many items it wants to send here */
case EVMSK_MAVLINK_IN_MISSION_COUNT:
/* this event now will be handled inside write loop */
chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
gcs2mav(mavlink_in_mission_count_struct.count);
/* register event back to main cycle */
chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
break;
/* ground wants erase all wps */
case EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL:
mission_clear_all();
break;
case EVMSK_MAVLINK_IN_MISSION_ITEM:
/* If a waypoint planner component receives WAYPOINT messages outside
* of transactions it answers with a WAYPOINT_ACK message. */
send_ack(MAV_MISSION_DENIED);
break;
default:
//chDbgPanic("unimplemented");
break;
}
}
chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
chEvtUnregister(&event_mavlink_in_mission_count, &el_mission_count);
chEvtUnregister(&event_mavlink_in_mission_clear_all, &el_mission_clear_all);
chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
chThdExit(0);
return 0;
}
void BaseThread::exit(msg_t msg) {
chThdExit(msg);
}
void osStartKernel(void)
{
//Terminate the main thread
chThdExit(RDY_OK);
}
NORETURN static void threadIO(void *tortilla) {
chRegSetThreadName("IO");
static_cast<Tortilla *>(tortilla)->ioLoop();
chThdExit(0);
}
msg_t EepromTestThread(void *sdp){
chRegSetThreadName("EepromTst");
cli_println("basic tests");
cli_println("--------------------------------------------------------------");
cli_print("mount aligned file sized to whole test area");
ocfg.barrier_low = TEST_AREA_START;
ocfg.barrier_hi = TEST_AREA_END;
EepromFileOpen(&ofile, &ocfg);
OK();
printfileinfo(sdp, &ofile);
cli_print("test fill with 0xFF");
pattern_fill(&ofile, 0xFF);
if (chThdShouldTerminate()){goto END;}
OK();
cli_print("test fill with 0xAA");
pattern_fill(&ofile, 0xAA);
if (chThdShouldTerminate()){goto END;}
OK();
cli_print("test fill with 0x55");
pattern_fill(&ofile, 0x55);
if (chThdShouldTerminate()){goto END;}
OK();
cli_print("test fill with 0x00");
pattern_fill(&ofile, 0x00);
if (chThdShouldTerminate()){goto END;}
OK();
cli_print("Closing file");
chFileStreamClose(&ofile);
OK();
uint32_t b1, b2, b3, b4, istart, ilen;
uint8_t pattern = 0x55;
b1 = TEST_AREA_START;
b2 = TEST_AREA_START + EEPROM_PAGE_SIZE;
b3 = TEST_AREA_END - EEPROM_PAGE_SIZE;
b4 = TEST_AREA_END;
istart = 0;
ilen = b3-b2;
cli_println(" Linear barriers testing.");
chThdSleepMilliseconds(20);
overflow_check( b1, b2, b3, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2, b3, b4, istart + 1, ilen - 1, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2, b3, b4, istart + 1, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2, b3, b4, istart + 1, ilen + 23, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b3 + 1, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 2, b3 + 2, b4, istart + 2, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b3 + 1, b4, istart + 1, ilen + 23, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 2, b3 + 2, b4, istart + 1, ilen + 23, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 + 2, b3 - 3, b4, istart + 2, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2, b2 + 1, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2, b2 + 2, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2, b2 + 3, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 + 1, b2 + 2, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 + 1, b2 + 3, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 + 1, b2 + 4, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b2, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b2 + 1, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b2 + 2, b4, istart, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b2 + 1, b4, istart + 1, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b2 + 2, b4, istart + 1, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
overflow_check( b1, b2 - 1, b2 + 3, b4, istart + 1, ilen, pattern, FALSE, sdp);
if (chThdShouldTerminate()){goto END;}
pattern++;
cli_println(" Basic API testing.");
chThdSleepMilliseconds(20);
ocfg.barrier_low = TEST_AREA_START;
ocfg.barrier_hi = TEST_AREA_END;
EepromFileOpen(&ofile, &ocfg);
chFileStreamSeek(&ofile, 0);
EepromWriteByte(&ofile, 0x11);
EepromWriteHalfword(&ofile, 0x2222);
EepromWriteWord(&ofile, 0x33333333);
chFileStreamSeek(&ofile, 0);
if(EepromReadByte(&ofile) != 0x11)
chDbgPanic("");
if(EepromReadHalfword(&ofile) != 0x2222)
chDbgPanic("");
if(EepromReadWord(&ofile) != 0x33333333)
chDbgPanic("");
chFileStreamClose(&ofile);
OK();
cli_println("All tests passed successfully.");
END:
chThdExit(0);
return 0;
}
static msg_t thdUsbStorage(void *arg)
{
(void) arg; // unused
chRegSetThreadName("UsbStorage:polling");
uint antiBounce=5;
EventListener connected;
// Should use EXTI interrupt instead of active polling,
// but in the chibios_opencm3 implementation, since EXTI is
// used via libopencm3, ISR are routed on pprz/opencm3 and cannot
// be used concurrently by chibios api
// Should be fixed when using chibios-rt branch
while (!chThdShouldTerminate() && antiBounce) {
const bool_t usbConnected = palReadPad (GPIOA, GPIOA_OTG_FS_VBUS);
if (usbConnected)
antiBounce--;
else
antiBounce=5;
chThdSleepMilliseconds(20);
}
isRunning = true;
chRegSetThreadName("UsbStorage:connected");
/* Stop the logs*/
chibios_logFinish (true);
/* connect sdcard sdc interface sdio */
if (sdioConnect () == false)
chThdExit (RDY_TIMEOUT);
/* initialize the USB mass storage driver */
msdInit(&UMSD1);
/* start the USB mass storage service */
msdStart(&UMSD1, &msdConfig);
/* start the USB driver */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(1000);
usbStart(&USBD1, &usbConfig);
usbConnectBus(&USBD1);
/* wait for a real usb storage connexion before shutting down autopilot */
chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1));
chEvtWaitOne(EVENT_MASK(1));
/* stop autopilot */
if (pprzThdPtr != NULL) {
chThdTerminate (pprzThdPtr);
chThdWait (pprzThdPtr);
pprzThdPtr = NULL;
}
/* wait until usb-storage is unmount and usb cable is unplugged*/
while (!chThdShouldTerminate() && palReadPad (GPIOA, GPIOA_OTG_FS_VBUS)) {
chThdSleepMilliseconds(10);
}
/* then close open descriptors and reboot autopilot */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(500);
msdStop(&UMSD1);
sdioDisconnect ();
MCU_RESTART();
return RDY_OK;
}
