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 insertion event.
*/
static void _insert_handler(void) {
bnapStorageObjectInit(&Storage, &MMCD1, mmcbuf);
bnapStorageStart(&Storage, &mmccfg);
/* On insertion SDC initialization and FS mount. */
if (CH_SUCCESS == bnapStorageConnect(&Storage)){
setGlobalFlag(GlobalFlags.storage_connected);
bnapStorageAcquire(&Storage);
bnapStorageMount(&Storage);
bnapStorageRelease(&Storage);
setGlobalFlag(GlobalFlags.logger_ready);
}
}
void MsgInit(void) {
chEvtInit(&event_gps_time_got);
chEvtInit(&event_mavlink_heartbeat_cc);
chEvtInit(&event_mavlink_heartbeat_mpiovd);
chEvtInit(&event_mavlink_heartbeat_dm);
chEvtInit(&event_mavlink_heartbeat_bnap);
chEvtInit(&event_mavlink_mpiovd_sensors);
chEvtInit(&event_mavlink_oblique_agps);
chEvtInit(&event_mavlink_oblique_rssi);
chEvtInit(&event_mavlink_oblique_storage_count);
chEvtInit(&event_mavlink_oblique_storage_request_count_cc);
chEvtInit(&event_mavlink_oblique_storage_request_cc);
chEvtInit(&event_mavlink_oblique_storage_request_count_dm);
chEvtInit(&event_mavlink_oblique_storage_request_dm);
chEvtInit(&event_mavlink_gps_raw_int);
chEvtInit(&event_mavlink_global_position_int);
chEvtInit(&event_mavlink_system_time);
chEvtInit(&event_mavlink_sys_status);
chEvtInit(&event_mavlink_statustext);
chEvtInit(&event_mavlink_command_long);
chEvtInit(&event_mavlink_param_value);
chEvtInit(&event_mavlink_param_set);
chEvtInit(&event_mavlink_param_request_list);
chEvtInit(&event_mavlink_param_request_read);
chEvtInit(&event_mavlink_command_ack);
chEvtInit(&event_mavlink_rc_channels_raw);
setGlobalFlag(GlobalFlags.messaging_ready);
}
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;
}
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;
}
/**
* обертка запускатор транзакции
*/
msg_t i2c_receive(i2caddr_t addr, uint8_t *rxbuf, size_t rxbytes){
msg_t status = RDY_OK;
i2cAcquireBus(&I2CD2);
status = i2cMasterReceiveTimeout(&I2CD2, addr, rxbuf, rxbytes, MS2ST(6));
i2cReleaseBus(&I2CD2);
chDbgAssert(status == RDY_OK, "i2c_transmit(), #1", "error in driver");
if (status == RDY_TIMEOUT){
/* в случае таймаута необходимо перезапустить драйвер */
i2cStop(&I2CD2);
chThdSleepMilliseconds(1);
i2cStart(&I2CD2, &i2cfg2);
setGlobalFlag(I2C_RESTARTED_FLAG);
return status;
}
return status;
}
/**
* обертка запускатор транзакции
*/
msg_t i2c_transmit(i2caddr_t addr, const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, size_t rxbytes){
msg_t status = RDY_OK;
i2cAcquireBus(&I2CD2);
status = i2cMasterTransmitTimeout(&I2CD2, addr, txbuf, txbytes, rxbuf, rxbytes, MS2ST(6));
i2cReleaseBus(&I2CD2);
if (status == RDY_TIMEOUT){
/* в случае таймаута необходимо перезапустить драйвер */
i2cStop(&I2CD2);
chThdSleepMilliseconds(1);
i2cStart(&I2CD2, &i2cfg2);
setGlobalFlag(I2C_RESTARTED_FLAG);
return status;
}
return status;
}
msg_t i2c_receive(i2caddr_t addr, uint8_t *rxbuf, size_t rxbytes){
msg_t status = RDY_OK;
systime_t tmo = calc_timeout(&I2C_BUS, 0, rxbytes);
i2cAcquireBus(&I2C_BUS);
status = i2cMasterReceiveTimeout(&I2C_BUS, addr, rxbuf, rxbytes, tmo);
i2cReleaseBus(&I2C_BUS);
chDbgAssert(status == RDY_OK, "i2c_transmit(), #1", "error in driver");
if (status == RDY_TIMEOUT){
//chprintf((BaseChannel *)&SD1, "I2C Timeout, restarting...\r\n");
i2cStop(&I2C_BUS);
chThdSleepMilliseconds(1);
i2cStart(&I2C_BUS, &i2cfg1);
setGlobalFlag(I2C_RESTARTED_FLAG);
return status;
}
return status;
}
msg_t i2c_transmit(i2caddr_t addr, const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, size_t rxbytes){
msg_t status = RDY_OK;
systime_t tmo = calc_timeout(&I2C_BUS, txbytes, rxbytes);
i2cAcquireBus(&I2C_BUS);
status = i2cMasterTransmitTimeout(&I2C_BUS, addr, txbuf, txbytes, rxbuf, rxbytes, tmo);
i2cReleaseBus(&I2C_BUS);
if (status == RDY_TIMEOUT){
//chprintf((BaseChannel *)&SD1, "I2C Timeout, restarting...\r\n");
i2cStop(&I2C_BUS);
chThdSleepMilliseconds(1);
i2cStart(&I2C_BUS, &i2cfg1);
setGlobalFlag(I2C_RESTARTED_FLAG);
return status;
}
return status;
}
/**
* Create telemetry link threads
*/
void SpawnMavlinkThreads(void *sdp) {
linkout_tp = chThdCreateStatic(LinkOutThreadWA,
sizeof(LinkOutThreadWA),
LINK_THREADS_PRIO,
LinkOutThread,
sdp);
if (linkout_tp == NULL)
chDbgPanic("Can not allocate memory");
linkin_tp = chThdCreateStatic(LinkInThreadWA,
sizeof(LinkInThreadWA),
LINK_THREADS_PRIO,
LinkInThread,
sdp);
if (linkin_tp == NULL)
chDbgPanic("Can not allocate memory");
setGlobalFlag(GlobalFlags.tlm_link_ready);
}
static msg_t LinkMgrThread(void *arg){
(void)arg;
chRegSetThreadName("LinkManager");
uint32_t sh = *sh_overxbee; // cached previouse value
/*
* 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(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/* wait slowpoked modems */
chThdSleepMilliseconds(3000);
/* define what we need to run based on flag */
if (sh == 1){
SpawnShellThreads(&XBEESD);
SpawnMavlinkThreads(&SDU1);
}
else{
SpawnShellThreads(&SDU1);
SpawnMavlinkThreads(&XBEESD);
}
/* say to all that modem is ready */
setGlobalFlag(GlobalFlags.modem_ready);
/* now track changes of flag and fork appropriate threads */
while (TRUE) {
chThdSleepMilliseconds(200);
if(sh != *sh_overxbee){ // state changed
sh = *sh_overxbee;
KillShellThreads();
KillMavlinkThreads();
chThdSleepMilliseconds(1);
// sdStop(&XBEESD);
// sdStart(&XBEESD, &xbee_ser_cfg);
//
// sduStop(&SDU1);
// sduStart(&SDU1, &serusbcfg);
// purge queues to be safer
// chSysLock();
// chIQResetI(&(SDU1.iqueue));
// chIQResetI(&(SDU1.oqueue));
// chIQResetI(&(XBEESD.iqueue));
// chIQResetI(&(XBEESD.oqueue));
// chSysUnlock();
// now spawn threads with proper serial drivers
if (sh == 1){
SpawnShellThreads(&XBEESD);
SpawnMavlinkThreads(&SDU1);
}
else{
SpawnShellThreads(&SDU1);
SpawnMavlinkThreads(&XBEESD);
}
}
}
return 0;
}