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; }