Exemplo n.º 1
0
static void cmdIr(BaseSequentialStream *chp, int argc, char *argv[])
{
    (void)argv;
    (void)argc;

    chEvtRegisterMask(&esSensorEvents , &el, SENSOR_EVENT_IR_END);

    chprintf(chp, "Pinging with IR...\r\n");

    chEvtBroadcastFlags(&esSensorEvents, SENSOR_EVENT_IR_START);
    chEvtWaitOne(SENSOR_EVENT_IR_END);

    chThdSleepMilliseconds(50);

    ir_index_t i;
    ir_angle_t angle;

    chSysLock();
    for (i = 0; i < IR_RX_COUNT; i++) {
        angle = irDetections[i];

        chprintf(chp, "IR Angle: %D\r\n", angle);
    }
    chSysUnlock();

    chEvtUnregister(&esSensorEvents, &el);
}
Exemplo n.º 2
0
Arquivo: autopilot.c Projeto: mcu786/u
/* прием и обработка комманд с земли*/
void process_cmd(mavlink_command_long_t *mavlink_command_long_struct){

  /* all this flags defined in MAV_CMD enum */
  switch(mavlink_command_long_struct->command){
  case MAV_CMD_DO_SET_MODE:
    /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
    mavlink_system_struct.mode = mavlink_command_long_struct->param1;
    command_accepted();
    break;

  /*
   * (пере)запуск калибровки
   */
  case MAV_CMD_PREFLIGHT_CALIBRATION:
     if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
      command_denied();
      return;
    }
    else{
      handle_calibration_cmd(mavlink_command_long_struct);
    }
    break;


  case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
    /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
    if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
      command_denied();
      return;
    }
    else{
      chEvtBroadcastFlags(&init_event, EVENT_MASK(SIGHALT_EVID));
      command_accepted();
    }
    break;
    /**
     * Команды для загрузки/вычитки параметров из EEPROM
     */
    case MAV_CMD_PREFLIGHT_STORAGE:
      if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT)
        return;

      if (mavlink_command_long_struct->param1 == 0)
        load_params_from_eeprom();
      else if (mavlink_command_long_struct->param1 == 1)
        save_params_to_eeprom();

      if (mavlink_command_long_struct->param2 == 0)
        load_mission_from_eeprom();
      else if (mavlink_command_long_struct->param2 == 1)
        save_mission_to_eeprom();

      break;

  default:
    return;
    break;
  }
}
Exemplo n.º 3
0
static void send_ack(uint8_t type){
  /* logically the target_component must be MAV_COMP_ID_MISSIONPLANNER,
   * but QGC does not accept them. */
  mavlink_out_mission_ack_struct.target_component = MAV_COMP_ID_ALL;
  //mavlink_out_mission_ack_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
  mavlink_out_mission_ack_struct.target_system = GROUND_STATION_ID;
  mavlink_out_mission_ack_struct.type = type;

  chEvtBroadcastFlags(&event_mavlink_out_mission_ack, EVMSK_MAVLINK_OUT_MISSION_ACK);
}
Exemplo n.º 4
0
/*
 * Application entry point.
 */
int main(void) {

	/*
	 * System initializations.
	 * - HAL initialization, this also initializes the configured device drivers
	 *   and performs the board-specific initializations.
	 * - Kernel initialization, the main() function becomes a thread and the
	 *   RTOS is active.
	 */
	halInit();
	chSysInit();

	chEvtInit(&eventImuIrq);
	chEvtInit(&eventMagnIrq);
	chEvtInit(&eventImuRead);
	chEvtInit(&eventMagnRead);
	chEvtInit(&eventEKFDone);

	palSetPadMode(GPIOB, 3, PAL_MODE_OUTPUT_PUSHPULL); // BLUE
	palSetPadMode(GPIOB, 4, PAL_MODE_OUTPUT_PUSHPULL); // GREEN
	palSetPadMode(GPIOB, 5, PAL_MODE_OUTPUT_PUSHPULL); // RED
	chThdCreateStatic(waThreadLed, sizeof(waThreadLed), NORMALPRIO, ThreadLed, NULL );

	I2CInitLocal();
	configInit();
	mavlinkInit();
	initSensors();

	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
	TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
	TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
	TIM_Cmd(TIM5, ENABLE);

	startEstimation();
	startSensors();
	radioInit();
	motorsInit();

	extStart(&EXTD1, &extcfg);
	extChannelEnable(&EXTD1, 0);
	extChannelEnable(&EXTD1, 1);

	chEvtBroadcastFlags(&eventEKFDone, EVT_EKF_DONE);

	while (TRUE) {
		chThdSleepMilliseconds(1000);
	}
}
Exemplo n.º 5
0
void motor_beep(int frequency, int duration_msec)
{
	chMtxLock(&_mutex);

	if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
		_state.beep_frequency = frequency;
		_state.beep_duration_msec = duration_msec;
		chMtxUnlock();
		chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS); // Wake the control thread
	} else {
		chMtxUnlock();
	}
}
Exemplo n.º 6
0
static msg_t PollIMUThread(void *arg){
	(void)arg;
	chRegSetThreadName("PollIMU");

	struct EventListener self_el;
	chEvtRegister(&imu_event, &self_el, 2);

	while (TRUE) {
		chEvtWaitAll(EVENT_MASK(0) && EVENT_MASK(4));
		mpu_i2c_read_data(0x3B, 14); // Read accelerometer, temperature and gyro data
		chEvtBroadcastFlags(&imu_event, EVENT_MASK(2));

	}
	return 0;
}
Exemplo n.º 7
0
static msg_t MmcReaderDmThread(void *sdp){
  chRegSetThreadName("MmcReaderDm");

  struct EventListener el_storage_request_count_dm;
  struct EventListener el_storage_request_dm;

  chEvtRegisterMask(&event_mavlink_oblique_storage_request_count_dm,
                    &el_storage_request_count_dm,
                    EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM);
  chEvtRegisterMask(&event_mavlink_oblique_storage_request_dm,
                    &el_storage_request_dm,
                    EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM);

  eventmask_t evt = 0;

  while (!chThdShouldTerminate()){
    evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM | EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM, MS2ST(50));
    if (!mmcIsCardInserted(&MMCD1))
      continue;
    else{
      switch (evt){
      case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM):
        bnapStorageAcquire(&Storage);
        mavlink_oblique_storage_count_struct.count = Storage.used;
        bnapStorageRelease(&Storage);
        chEvtBroadcastFlags(&event_mavlink_oblique_storage_count, EVMSK_MAVLINK_OBLIQUE_STORAGE_COUNT);
        break;

      case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM):
        _oblique_storage_request_handler_dm(sdp);
        break;

      default:
        break;
      }
    }
  }

  chEvtUnregister(&event_mavlink_oblique_storage_request_count_dm,
                  &el_storage_request_count_dm);
  chEvtUnregister(&event_mavlink_oblique_storage_request_dm,
                  &el_storage_request_dm);
  return 0;
}
Exemplo n.º 8
0
void motor_set_duty_cycle(float dc, int ttl_ms)
{
	chMtxLock(&_mutex);

	_state.mode = MOTOR_CONTROL_MODE_OPENLOOP;

	if (dc < 0.0) { dc = 0.0; }
	if (dc > 1.0) { dc = 1.0; }
	_state.dc_openloop_setpoint = dc;
	_state.setpoint_ttl_ms = ttl_ms;

	if (dc == 0.0) {
		_state.num_unexpected_stops = 0;
	}

	chMtxUnlock();

	// Wake the control thread to process the new setpoint immediately
	chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS);
}
Exemplo n.º 9
0
void motor_set_rpm(unsigned rpm, int ttl_ms)
{
	chMtxLock(&_mutex);

	_state.mode = MOTOR_CONTROL_MODE_RPM;

	if (rpm > _params.rpm_max) {
		rpm = _params.rpm_max;
	}
	_state.rpm_setpoint = rpm;
	_state.setpoint_ttl_ms = ttl_ms;

	if (rpm == 0) {
		_state.num_unexpected_stops = 0;
	}

	chMtxUnlock();

	// Wake the control thread to process the new setpoint immediately
	chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS);
}
Exemplo n.º 10
0
/**
 * @details    When the last waypoint was successfully received
 *             the requesting component sends a WAYPOINT_ACK message
 *             to the targeted component. This finishes the transaction.
 *             Notice that the targeted component has to listen to
 *             WAYPOINT_REQUEST messages of the last waypoint until it
 *             gets the WAYPOINT_ACK or another message that starts
 *             a different transaction or a timeout happens.
 */
static bool mav2gcs(void){
  eventmask_t evt = 0;
  bool status = CH_FAILED;
  uint32_t retry_cnt = PLANNER_RETRY_CNT;

  struct EventListener el_mission_request;
  struct EventListener el_mission_ack;
  struct EventListener el_mission_request_list;
  chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
  chEvtRegisterMask(&event_mavlink_in_mission_request,      &el_mission_request,      EVMSK_MAVLINK_IN_MISSION_REQUEST);
  chEvtRegisterMask(&event_mavlink_in_mission_ack,          &el_mission_ack,          EVMSK_MAVLINK_IN_MISSION_ACK);

START:
  mavlink_out_mission_count_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
  mavlink_out_mission_count_struct.target_system = GROUND_STATION_ID;
  mavlink_out_mission_count_struct.count = wpdb.getCount();

  chEvtBroadcastFlags(&event_mavlink_out_mission_count, EVMSK_MAVLINK_OUT_MISSION_COUNT);

  if (0 == mavlink_out_mission_count_struct.count){
    status = CH_SUCCESS;
    goto EXIT;
  }


  /* теперь нам надо понять, дошло сообщение с количеством вейпоинтов, или нет.
   * Если нет - земля пришле повторный запрос MISSION_REQUEST_LIST */
  evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST |
                            EVMSK_MAVLINK_IN_MISSION_REQUEST |
                            EVMSK_MAVLINK_IN_MISSION_ACK,
                            PLANNER_RETRY_TMO * PLANNER_RETRY_CNT);
  switch(evt){
  case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST:
    /* надобно повторить */
    goto START;

  /* maint loooong loop */
  case EVMSK_MAVLINK_IN_MISSION_REQUEST:
    do{
      wpdb.load(&mavlink_out_mission_item_struct, mavlink_in_mission_request_struct.seq);
      mavlink_out_mission_item_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
      mavlink_out_mission_item_struct.target_system = GROUND_STATION_ID;
      chEvtBroadcastFlags(&event_mavlink_out_mission_item, EVMSK_MAVLINK_OUT_MISSION_ITEM);
      /* wait next request or ack */
      evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST |
                                EVMSK_MAVLINK_IN_MISSION_ACK,
                                PLANNER_RETRY_TMO);
      switch(evt){
      case EVMSK_MAVLINK_IN_MISSION_REQUEST:
        continue;

      case EVMSK_MAVLINK_IN_MISSION_ACK:
        status = CH_SUCCESS;
        goto EXIT;
        break;

      default:
        retry_cnt--;
        status = CH_FAILED;
        break;
      }
    }while(retry_cnt);
    break; /* case EVMSK_MAVLINK_IN_MISSION_REQUEST */

  case EVMSK_MAVLINK_IN_MISSION_ACK:
    status = CH_SUCCESS;
    break;

  default:
    status = CH_FAILED;
    break;
  }

EXIT:
  chEvtUnregister(&event_mavlink_in_mission_request, &el_mission_request);
  chEvtUnregister(&event_mavlink_in_mission_ack,     &el_mission_ack);
  chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
  return status;
}
Exemplo n.º 11
0
static bool gcs2mav(uint16_t N){
  uint32_t seq = 0;
  uint32_t retry_cnt = PLANNER_RETRY_CNT;
  eventmask_t evt = 0;
  mavlink_mission_item_t mi;        /* local copy */
  uint8_t status = MAV_MISSION_ERROR;

  /* check available space */
  if (N > wpdb.getCapacity()){
    send_ack(MAV_MISSION_NO_SPACE);
    return CH_FAILED;
  }

  /* prepare to harvest waypoints */
  struct EventListener el_mission_item;
  chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
  chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, 1);/* fake wait to clear "queue" */
  if (CH_FAILED == wpdb.clear())
    chDbgPanic("");

  for (seq=0; seq<N; seq++){
    /* prepare request to ground */
    mavlink_out_mission_request_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
    mavlink_out_mission_request_struct.target_system = GROUND_STATION_ID;
    mavlink_out_mission_request_struct.seq = seq;
    retry_cnt = PLANNER_RETRY_CNT;
    chThdSleep(PLANNER_ADDITIONAL_TMO);

    do{
      /* drop message */
      chEvtBroadcastFlags(&event_mavlink_out_mission_request, EVMSK_MAVLINK_OUT_MISSION_REQUEST);

      /* wait answer */
      evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, PLANNER_RETRY_TMO);
      if (EVMSK_MAVLINK_IN_MISSION_ITEM == evt){
        chSysLock();
        mi = mavlink_in_mission_item_struct;
        chSysUnlock();

        /* check waypoint cosherness and write it if cosher */
        status = check_wp(&mi, seq);
        if (status != MAV_MISSION_ACCEPTED)
          goto EXIT;
        else{
          if (CH_FAILED == wpdb.save(&mi, seq))
            chDbgPanic("");
          break; /* do-while */
        }
      }

      retry_cnt--;
      if(0 == retry_cnt)
        goto EXIT;

    }while(retry_cnt);
  }
  /* save waypoint count in eeprom only in the very end of transaction */
  if (CH_FAILED == wpdb.finalize())
    chDbgPanic("");

  /* final stuff */
EXIT:
  chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
  send_ack(status);
  if (0 == retry_cnt)
    return CH_FAILED;
  else
    return CH_SUCCESS;
}
Exemplo n.º 12
0
void EvtSource::broadcastFlags(flagsmask_t flags) {

    chEvtBroadcastFlags(&ev_source, flags);
}
Exemplo n.º 13
0
void CcUnpackCycle(SerialDriver *sdp){
  mavlink_message_t msg;
  mavlink_status_t status;
  msg_t c = 0;
  msg_t prev_c = 0;

  while (!chThdShouldTerminate()) {
    if (!cc_port_ready()){
      chThdSleepMilliseconds(50);
      continue;
    }
    else{
      /* Try to get an escaped with DLE symbols message */
      c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50));
      if (c == Q_TIMEOUT)
        continue;
      prev_c = c;
      if (prev_c == DLE){
        prev_c = 0; /* set it to any just not DLE nor ETX */
        c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50));
        if (c == Q_TIMEOUT)
          continue;
      }
    }

    if (mavlink_parse_char(MAVLINK_COMM_0, (uint8_t)c, &msg, &status)) {
      switch(msg.msgid){
        case MAVLINK_MSG_ID_COMMAND_LONG:
          mavlink_msg_command_long_decode(&msg, &mavlink_command_long_struct);
          if (mavlink_command_long_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_command_long, EVMSK_MAVLINK_COMMAND_LONG);
          break;

        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
          mavlink_msg_param_request_read_decode(&msg, &mavlink_param_request_read_struct);
          if (mavlink_param_request_read_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_param_request_read, EVMSK_MAVLINK_PARAM_REQUEST_READ);
          break;

        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
          mavlink_msg_param_request_list_decode(&msg, &mavlink_param_request_list_struct);
          if (mavlink_param_request_list_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_param_request_list, EVMSK_MAVLINK_PARAM_REQUEST_LIST);
          break;

        case MAVLINK_MSG_ID_PARAM_SET:
          mavlink_msg_param_set_decode(&msg, &mavlink_param_set_struct);
          if (mavlink_param_set_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_param_set, EVMSK_MAVLINK_PARAM_SET);
          break;

        case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_CC:
          mavlink_msg_oblique_storage_request_cc_decode(&msg, &mavlink_oblique_storage_request_cc_struct);
          if (mavlink_oblique_storage_request_cc_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_CC);
          break;

        case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_COUNT_CC:
          mavlink_msg_oblique_storage_request_count_cc_decode(&msg, &mavlink_oblique_storage_request_count_cc_struct);
          if (mavlink_oblique_storage_request_count_cc_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_count_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_CC);
          break;

        case MAVLINK_MSG_ID_HEARTBEAT_CC:
          mavlink_msg_heartbeat_cc_decode(&msg, &mavlink_heartbeat_cc_struct);
//          if (mavlink_heartbeat_cc_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_heartbeat_cc, EVMSK_MAVLINK_HEARTBEAT_CC);
          break;

        case MAVLINK_MSG_ID_STATUSTEXT:
          mavlink_msg_statustext_decode(&msg, &mavlink_statustext_struct);
//          if (mavlink_statustext_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_statustext, EVMSK_MAVLINK_STATUSTEXT);
          break;

        case MAVLINK_MSG_ID_OBLIQUE_AGPS:
          mavlink_msg_oblique_agps_decode(&msg, &mavlink_oblique_agps_struct);
          if (mavlink_oblique_agps_struct.target_system == mavlink_system_struct.sysid)
            chEvtBroadcastFlags(&event_mavlink_oblique_agps, EVMSK_MAVLINK_OBLIQUE_AGPS);
          break;


      default:
        break;
      }
    }
  }
}
Exemplo n.º 14
0
/**
 * @brief Main estimation thread.
 *
 * @param[in/out] arg   Unused.
 */
static THD_FUNCTION(ThreadEstimation, arg)
{
    (void)arg;

    chRegSetThreadName("Estimation");

    tp = chThdGetSelfX();

    /* Initialization data */
    //uint32_t i;
    //quaternion_t q_init = {1.0f, 0.0f, 0.0f, 0.0f};
    //vector3f_t am, wb_init = {0.0f, 0.0f, 0.0f}; //, acc_init, mag_init;

    /* Event registration for new sensor data */
    event_listener_t el;

    /* Register to new data from accelerometer and gyroscope */
    chEvtRegisterMaskWithFlags(ptrGetNewDataEventSource(),
                      &el,
                      EVENT_MASK(0),
                      ACCGYRO_DATA_AVAILABLE_EVENTMASK |
                      MAG_DATA_AVAILABLE_EVENTMASK |
                      BARO_DATA_AVAILABLE_EVENTMASK);

    /* Force an initialization of the estimation */
    //chEvtAddEvents(ESTIMATION_RESET_EVENTMASK);

    //AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
    vInitializeMotionCaptureEstimator(&states);

    while(1)
    {
        //if (isnan(states.q.q0) || isnan(states.q.q1) || isnan(states.q.q2) || isnan(states.q.q3) ||
        //    isnan(states.w.x) || isnan(states.w.y) || isnan(states.w.z) ||
        //    isnan(states.wb.x) || isnan(states.wb.y) || isnan(states.wb.z))
        //    AttitudeEstimationInit(&states, &data, &q_init, &wb_init);

        /* Check if there has been a request to reset the filter */
        //if (chEvtWaitOneTimeout(ESTIMATION_RESET_EVENTMASK, TIME_IMMEDIATE))
       // {
            /* Initialize the estimation */
            //AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
        //}

        /* Wait for new measurement data */
        chEvtWaitAny(ALL_EVENTS);

        eventflags_t flags = chEvtGetAndClearFlags(&el);

        if (flags & ACCGYRO_DATA_AVAILABLE_EVENTMASK)
        {

            /* Get sensor data */
            GetIMUData(&imu_data);

            /* Run estimation */
            vInnovateMotionCaptureEstimator(&states,
                                            &imu_data,
                                            SENSOR_ACCGYRO_DT,
                                            0.0007f);

            /*InnovateAttitudeEKF(&states,
                                &data,
                                imu_data.gyroscope,
                                imu_data.accelerometer,
                                imu_data.magnetometer,
                                0.0f,
                                0.0f,
                                ESTIMATION_DT);*/

            //states.w.x = -imu_data.gyroscope[0];
            //states.w.y = -imu_data.gyroscope[1];
            //states.w.z = imu_data.gyroscope[2];

            //am.x = -imu_data.accelerometer[0];
            //am.y = -imu_data.accelerometer[1];
            //am.z = imu_data.accelerometer[2];

            //states.q = MadgwickAHRSupdateIMU(states.w,
            //                                 am,
            //                                 states.q,
            //                                 0.15f,
            //                                 dt);

            /* Broadcast new estimation available */
            chEvtBroadcastFlags(&estimation_events_es,
                                ESTIMATION_NEW_ESTIMATION_EVENTMASK);

        }
    }
}
Exemplo n.º 15
0
  void EvtSource::broadcastFlags(eventflags_t flags) {

    chEvtBroadcastFlags(&ev_source, flags);
  }