void handle_calibration_cmd(mavlink_command_long_t *mavlink_command_long_struct){
/* Trigger calibration. This command will be only accepted if in pre-flight mode.
* | Gyro calibration: 0: no, 1: yes
* | Magnetometer calibration: 0: no, 1: yes
* | Ground pressure: 0: no, 1: yes
* | Radio calibration: 0: no, 1: yes
* | Empty| Empty| Empty| */
/* Gyro */
if (mavlink_command_long_struct->param1 == 1){
setGlobalFlag(GYRO_CAL_FLAG);
mavlink_system_struct.state = MAV_STATE_CALIBRATING;
}
else{
clearGlobalFlag(GYRO_CAL_FLAG);
mavlink_system_struct.state = MAV_STATE_STANDBY;
}
/* Magnetometer */
if (mavlink_command_long_struct->param2 == 1){
setGlobalFlag(MAG_CAL_FLAG);
mavlink_system_struct.state = MAV_STATE_CALIBRATING;
}
else{
clearGlobalFlag(MAG_CAL_FLAG);
mavlink_system_struct.state = MAV_STATE_STANDBY;
}
}
/*
* SD card removal event.
*/
static void _remove_handler(void) {
clearGlobalFlag(GlobalFlags.logger_ready);
clearGlobalFlag(GlobalFlags.storage_connected);
if (Storage.mmcp->state == BLK_ACTIVE){
bnapStorageDisconnect(&Storage);
bnapStorageStop(&Storage);
}
}
static msg_t SdThread(void *arg){
chRegSetThreadName("MicroSD");
(void)arg;
FRESULT err;
uint32_t clusters;
FATFS *fsp;
FIL Log;
msg_t id;
/* wait until card not ready */
NOT_READY:
while (!sdcIsCardInserted(&SDCD1))
chThdSleepMilliseconds(SDC_POLLING_INTERVAL);
chThdSleepMilliseconds(SDC_POLLING_INTERVAL);
if (!sdcIsCardInserted(&SDCD1))
goto NOT_READY;
else
insert_handler();
/* fs mounted? */
if (!fs_ready)
return RDY_RESET;
/* are we have at least 16MB of free space? */
err = f_getfree("/", &clusters, &fsp);
err_check();
if ((clusters * (uint32_t)SDC_FS.csize * (uint32_t)MMCSD_BLOCK_SIZE) < (16*1024*1024))
return RDY_RESET;
/* open file for writing log */
char namebuf[MAX_FILENAME_SIZE];
name_from_time(namebuf);
err = f_open(&Log, namebuf, FA_WRITE | FA_CREATE_ALWAYS);
err_check();
/* main write cycle
* This writer waits msg_t with mavlink message ID. Based on that ID it
* will pack extern mavlink struct with proper packing function. */
setGlobalFlag(GlobalFlags.logger_ready);
while (TRUE){
/* wait ID */
if (logwriter_mb.fetch(&id, TIME_INFINITE) == RDY_OK){
if (!sdcIsCardInserted(&SDCD1)){
clearGlobalFlag(GlobalFlags.logger_ready);
remove_handler();
goto NOT_READY;
}
err = WriteLog(&Log, id, &fresh_data);
err_check();
}
err = fs_sync(&Log);
err_check();
}
return 0;
}
/**
* Kills previously spawned threads
*/
void KillMavlinkThreads(void) {
clearGlobalFlag(GlobalFlags.tlm_link_ready);
chThdTerminate(linkout_tp);
chThdTerminate(linkin_tp);
chThdWait(linkout_tp);
chThdWait(linkin_tp);
}
static msg_t UsbCanMgrThread(void *arg){
(void)arg;
Thread* curr_tp = NULL;
chRegSetThreadName("UsbCanManager");
/* use CAN as starting point by default */
setGlobalFlag(CAN_ACTIVE_FLAG);
/* now track changes of flag and fork appropriate threads */
while (TRUE) {
chThdSleepMilliseconds(100);
if (curr_tp != NULL){
if(is_usb_present() && (GlobalFlags & CAN_ACTIVE_FLAG)){
term_can_tread(curr_tp);
clearGlobalFlag(CAN_ACTIVE_FLAG);
curr_tp = fork_usb_tread();
}
else if(!is_usb_present() && !(GlobalFlags & CAN_ACTIVE_FLAG)){
term_usb_tread(curr_tp);
curr_tp = NULL;
setGlobalFlag(CAN_ACTIVE_FLAG);
curr_tp = fork_can_tread();
}
}
/* initial thread fork */
else{
if(is_usb_present()){
clearGlobalFlag(CAN_ACTIVE_FLAG);
curr_tp = fork_usb_tread();
}
else {
setGlobalFlag(CAN_ACTIVE_FLAG);
curr_tp = fork_can_tread();
}
}
}
return 0;
}
