Exemple #1
0
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;
  }
}
Exemple #2
0
/*
 * 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);
  }
}
Exemple #3
0
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);
}
Exemple #4
0
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;
}
Exemple #5
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;
}
Exemple #6
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;
}
Exemple #7
0
/**
 * обертка запускатор транзакции
 */
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;
}
Exemple #10
0
/**
 * 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);
}
Exemple #11
0
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;
}