Exemplo n.º 1
0
static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
{
#if defined RADIO_CONTROL
  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
                               get_sys_time_msec(),
                               RC_CHANNELS,
                               PPM_PULSES(0), PPM_PULSES(1), PPM_PULSES(2),
                               PPM_PULSES(3), PPM_PULSES(4), PPM_PULSES(5),
                               PPM_PULSES(6), PPM_PULSES(7), PPM_PULSES(8),
                               PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
                               PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
                               PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
                               255); // rssi unknown
#else
  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
                               get_sys_time_msec(),
                               0, // zero channels available
                               UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
                               UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
                               UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
                               UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
                               UINT16_MAX, UINT16_MAX, 255);
#endif
  MAVLinkSendMessage();
}
Exemplo n.º 2
0
static uint8_t gps_multi_switch(struct GpsState *gps_s) {
  static uint32_t time_since_last_gps_switch = 0;

  if (multi_gps_mode == GPS_MODE_PRIMARY){
    return GpsInstances[PRIMARY_GPS_INSTANCE].id;
  } else if (multi_gps_mode == GPS_MODE_SECONDARY){
    return GpsInstances[SECONDARY_GPS_INSTANCE].id;
  } else{
    if (gps_s->fix > gps.fix){
      return gps_s->comp_id;
    } else if (gps.fix > gps_s->fix){
      return gps.comp_id;
    } else{
      if (get_sys_time_msec() - time_since_last_gps_switch > TIME_TO_SWITCH) {
        if (gps_s->num_sv > gps.num_sv) {
          current_gps_id = gps_s->comp_id;
          time_since_last_gps_switch = get_sys_time_msec();
        } else if (gps.num_sv > gps_s->num_sv) {
          current_gps_id = gps.comp_id;
          time_since_last_gps_switch = get_sys_time_msec();
        }
      }
    }
  }
  return current_gps_id;
}
Exemplo n.º 3
0
static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
                                      get_sys_time_msec(),
                                      stateGetPositionNed_f()->x,
                                      stateGetPositionNed_f()->y,
                                      stateGetPositionNed_f()->z,
                                      stateGetSpeedNed_f()->x,
                                      stateGetSpeedNed_f()->y,
                                      stateGetSpeedNed_f()->z);
  MAVLinkSendMessage();
}
Exemplo n.º 4
0
/**
 * Send the attitude
 */
static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_attitude_send(MAVLINK_COMM_0,
                            get_sys_time_msec(),
                            stateGetNedToBodyEulers_f()->phi,     // Phi
                            stateGetNedToBodyEulers_f()->theta,   // Theta
                            stateGetNedToBodyEulers_f()->psi,     // Psi
                            stateGetBodyRates_f()->p,             // p
                            stateGetBodyRates_f()->q,             // q
                            stateGetBodyRates_f()->r);            // r
  MAVLinkSendMessage();
}
Exemplo n.º 5
0
static inline void mavlink_send_local_position_ned(void)
{
  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
                                      get_sys_time_msec(),
                                      stateGetPositionNed_f()->x,
                                      stateGetPositionNed_f()->y,
                                      stateGetPositionNed_f()->z,
                                      stateGetSpeedNed_f()->x,
                                      stateGetSpeedNed_f()->y,
                                      stateGetSpeedNed_f()->z);
  MAVLinkSendMessage();
}
Exemplo n.º 6
0
/**
 * Send the attitude
 */
static inline void mavlink_send_attitude(void)
{
  mavlink_msg_attitude_send(MAVLINK_COMM_0,
                            get_sys_time_msec(),
                            stateGetNedToBodyEulers_f()->phi,     // Phi
                            stateGetNedToBodyEulers_f()->theta,   // Theta
                            stateGetNedToBodyEulers_f()->psi,     // Psi
                            stateGetBodyRates_f()->p,             // p
                            stateGetBodyRates_f()->q,             // q
                            stateGetBodyRates_f()->r);            // r
  MAVLinkSendMessage();
}
Exemplo n.º 7
0
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
                                       get_sys_time_msec(),
                                       stateGetNedToBodyQuat_f()->qi,
                                       stateGetNedToBodyQuat_f()->qx,
                                       stateGetNedToBodyQuat_f()->qy,
                                       stateGetNedToBodyQuat_f()->qz,
                                       stateGetBodyRates_f()->p,
                                       stateGetBodyRates_f()->q,
                                       stateGetBodyRates_f()->r);
  MAVLinkSendMessage();
}
Exemplo n.º 8
0
static inline void mavlink_send_attitude_quaternion(void)
{
  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
                                       get_sys_time_msec(),
                                       stateGetNedToBodyQuat_f()->qi,
                                       stateGetNedToBodyQuat_f()->qx,
                                       stateGetNedToBodyQuat_f()->qy,
                                       stateGetNedToBodyQuat_f()->qz,
                                       stateGetBodyRates_f()->p,
                                       stateGetBodyRates_f()->q,
                                       stateGetBodyRates_f()->r);
  MAVLinkSendMessage();
}
Exemplo n.º 9
0
static void send_downlink(struct transport_tx *trans, struct link_device *dev) {
  static uint32_t last_nb_bytes = 0;
  // timestamp in usec when last message was send
  static uint32_t last_ts = 0.;
  // current timestamp
  uint32_t now_ts = get_sys_time_msec();
  // compute downlink byte rate
  if (now_ts > last_ts) {
    uint16_t rate = (1000 * ((uint32_t)downlink.nb_bytes - last_nb_bytes)) / (now_ts - last_ts);
    last_ts = now_ts;
    last_nb_bytes = downlink.nb_bytes;

    // TODO uplink nb received msg
    uint16_t uplink_nb_msgs = 0;
    pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID,
        &datalink_time, &uplink_nb_msgs,
        &downlink.nb_msgs, &rate, &downlink.nb_ovrn);
  }
}
Exemplo n.º 10
0
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
{
  float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
  if (heading < 0.) {
    heading += 360;
  }
  uint16_t compass_heading = heading * 100;
  int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;
  /// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs
  mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
                                       get_sys_time_msec(),
                                       stateGetPositionLla_i()->lat,
                                       stateGetPositionLla_i()->lon,
                                       stateGetPositionLla_i()->alt,
                                       relative_alt,
                                       stateGetSpeedNed_f()->x * 100,
                                       stateGetSpeedNed_f()->y * 100,
                                       stateGetSpeedNed_f()->z * 100,
                                       compass_heading);
  MAVLinkSendMessage();
}
Exemplo n.º 11
0
/**
 * Send SYSTEM_TIME
 * - time_unix_usec
 * - time_boot_ms
 */
static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
  MAVLinkSendMessage();
}
Exemplo n.º 12
0
/**
 * Send SYSTEM_TIME
 * - time_unix_usec
 * - time_boot_ms
 */
static inline void mavlink_send_system_time(void)
{
  mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
  MAVLinkSendMessage();
}
Exemplo n.º 13
0
void periodic_2Hz_openlog(void)
{
  uint32_t timestamp = get_sys_time_msec();
  DOWNLINK_SEND_TIMESTAMP(DefaultChannel, DefaultDevice, &timestamp);
}
Exemplo n.º 14
0
  void adc_isr(void)
#endif
{
  uint8_t channel = 0;
  uint16_t value  = 0;
  struct adc_buf * buf;

#if USE_ADC_WATCHDOG
  /*
    We need adc sampling fast enough to detect battery plug out, but we did not
    need to get actual actual value so fast. So timer fire adc conversion fast,
    at least 500 hz, but we inject adc value in sampling buffer only at 50hz
   */
  const uint32_t timeStampDiff = get_sys_time_msec() - adc_watchdog.timeStamp;
  const bool_t shouldAccumulateValue = timeStampDiff > 20;
  if (shouldAccumulateValue)
    adc_watchdog.timeStamp = get_sys_time_msec();

  if (adc_watchdog.cb != NULL) {
    if (adc_awd(adc_watchdog.adc)) {
      ADC_SR(adc_watchdog.adc) &= ~ADC_SR_AWD; // clear int flag
      adc_watchdog.cb();
    }
  }
#endif

#if USE_AD1
  // Clear Injected End Of Conversion
  if (adc_eoc_injected(ADC1)){
    ADC_SR(ADC1) &= ~ADC_SR_JEOC;
#if USE_ADC_WATCHDOG
    if (shouldAccumulateValue) {
#endif
    for (channel = 0; channel < nb_adc1_channels; channel++) {
      buf = adc1_buffers[channel];
      if (buf) {
        value = adc_read_injected(ADC1, channel+1);
        adc_push_sample(buf, value);
      }
    }
#if USE_ADC_WATCHDOG
  }
#endif

#if !USE_AD2 && !USE_AD3
    adc_new_data_trigger = TRUE;
#endif
  }
#endif
#if USE_AD2
  if (adc_eoc_injected(ADC2)){
    ADC_SR(ADC2) &= ~ADC_SR_JEOC;
#if USE_ADC_WATCHDOG
    if (shouldAccumulateValue) {
#endif
    for (channel = 0; channel < nb_adc2_channels; channel++) {
      buf = adc2_buffers[channel];
      if (buf) {
        value = adc_read_injected(ADC2, channel+1);
        adc_push_sample(buf, value);
      }
    }
#if USE_ADC_WATCHDOG
  }
#endif
#if !USE_AD3
    adc_new_data_trigger = TRUE;
#endif
  }
#endif
#if USE_AD3
  if (adc_eoc_injected(ADC3)){
    ADC_SR(ADC3) &= ~ADC_SR_JEOC;
#if USE_ADC_WATCHDOG
    if (shouldAccumulateValue) {
#endif
    for (channel = 0; channel < nb_adc3_channels; channel++) {
      buf = adc3_buffers[channel];
      if (buf) {
        value = adc_read_injected(ADC3, channel+1);
        adc_push_sample(buf, value);
      }
    }
#if USE_ADC_WATCHDOG
  }
#endif
    adc_new_data_trigger = TRUE;
  }
#endif


  return;
}