Exemplo n.º 1
0
void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
{
  uint8_t sender_id = SenderIdOfPprzMsg(buf);
  uint8_t msg_id = IdOfPprzMsg(buf);

  /* parse telemetry messages coming from other AC */
  if (sender_id != 0) {
    switch (msg_id) {
      default: {
        break;
      }
    }
  } else {
    /* parse telemetry messages coming from ground station */
    switch (msg_id) {
      case  DL_PING: {
        pprz_msg_send_PONG(trans, dev, AC_ID);
      }
      break;

      case DL_SETTING : {
        if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
        uint8_t i = DL_SETTING_index(buf);
        float var = DL_SETTING_value(buf);
        DlSetting(i, var);
        pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
      }
      break;

      case DL_GET_SETTING : {
        if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
        uint8_t i = DL_GET_SETTING_index(buf);
        float val = settings_get_value(i);
        pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
      }
      break;

#ifdef RADIO_CONTROL_TYPE_DATALINK
      case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
        LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
        parse_rc_3ch_datalink(
          DL_RC_3CH_throttle_mode(buf),
          DL_RC_3CH_roll(buf),
          DL_RC_3CH_pitch(buf));
        break;
      case DL_RC_4CH :
        if (DL_RC_4CH_ac_id(buf) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
          LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
          parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
                                DL_RC_4CH_throttle(buf),
                                DL_RC_4CH_roll(buf),
                                DL_RC_4CH_pitch(buf),
                                DL_RC_4CH_yaw(buf));
        }
        break;
#endif // RADIO_CONTROL_TYPE_DATALINK

#if USE_GPS
      case DL_GPS_INJECT : {
        // Check if the GPS is for this AC
        if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }

        // GPS parse data
        gps_inject_data(
          DL_GPS_INJECT_packet_id(buf),
          DL_GPS_INJECT_data_length(buf),
          DL_GPS_INJECT_data(buf)
        );
      }
      break;
#endif  // USE_GPS

      default:
        break;
    }
  }
  /* Parse firmware specific datalink */
  firmware_parse_msg(dev, trans, buf);

  /* Parse modules datalink */
  modules_parse_datalink(msg_id);
}
Exemplo n.º 2
0
void dl_parse_msg(void)
{

  uint8_t msg_id = IdOfMsg(dl_buffer);
  switch (msg_id) {

    case  DL_PING: {
      DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
    }
    break;

    case DL_SETTING : {
      if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
      uint8_t i = DL_SETTING_index(dl_buffer);
      float var = DL_SETTING_value(dl_buffer);
      DlSetting(i, var);
      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
    }
    break;

    case DL_GET_SETTING : {
      if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
      uint8_t i = DL_GET_SETTING_index(dl_buffer);
      float val = settings_get_value(i);
      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
    }
    break;

#ifdef USE_NAVIGATION
    case DL_BLOCK : {
      if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
      nav_goto_block(DL_BLOCK_block_id(dl_buffer));
    }
    break;

    case DL_MOVE_WP : {
      uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
      if (ac_id != AC_ID) { break; }
      if (stateIsLocalCoordinateValid()) {
        uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
        struct LlaCoor_i lla;
        lla.lat = DL_MOVE_WP_lat(dl_buffer);
        lla.lon = DL_MOVE_WP_lon(dl_buffer);
        /* WP_alt from message is alt above MSL in mm
         * lla.alt is above ellipsoid in mm
         */
        lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
                  state.ned_origin_i.lla.alt;
        waypoint_move_lla(wp_id, &lla);
      }
    }
    break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
    case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
      LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
      parse_rc_3ch_datalink(
        DL_RC_3CH_throttle_mode(dl_buffer),
        DL_RC_3CH_roll(dl_buffer),
        DL_RC_3CH_pitch(dl_buffer));
      break;
    case DL_RC_4CH :
      if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
        LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
        parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
                              DL_RC_4CH_throttle(dl_buffer),
                              DL_RC_4CH_roll(dl_buffer),
                              DL_RC_4CH_pitch(dl_buffer),
                              DL_RC_4CH_yaw(dl_buffer));
      }
      break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if USE_GPS
#ifdef GPS_DATALINK
    case DL_REMOTE_GPS_SMALL : {
      // Check if the GPS is for this AC
      if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }

      parse_gps_datalink_small(
        DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
        DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
        DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
        DL_REMOTE_GPS_SMALL_heading(dl_buffer));
    }
    break;

    case DL_REMOTE_GPS : {
      // Check if the GPS is for this AC
      if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }

      // Parse the GPS
      parse_gps_datalink(
        DL_REMOTE_GPS_numsv(dl_buffer),
        DL_REMOTE_GPS_ecef_x(dl_buffer),
        DL_REMOTE_GPS_ecef_y(dl_buffer),
        DL_REMOTE_GPS_ecef_z(dl_buffer),
        DL_REMOTE_GPS_lat(dl_buffer),
        DL_REMOTE_GPS_lon(dl_buffer),
        DL_REMOTE_GPS_alt(dl_buffer),
        DL_REMOTE_GPS_hmsl(dl_buffer),
        DL_REMOTE_GPS_ecef_xd(dl_buffer),
        DL_REMOTE_GPS_ecef_yd(dl_buffer),
        DL_REMOTE_GPS_ecef_zd(dl_buffer),
        DL_REMOTE_GPS_tow(dl_buffer),
        DL_REMOTE_GPS_course(dl_buffer));
    }
    break;
#endif // GPS_DATALINK

    case DL_GPS_INJECT : {
      // Check if the GPS is for this AC
      if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }

      // GPS parse data
      gps_inject_data(
        DL_GPS_INJECT_packet_id(dl_buffer),
        DL_GPS_INJECT_data_length(dl_buffer),
        DL_GPS_INJECT_data(dl_buffer)
      );
    }
    break;
#endif  // USE_GPS

    case DL_GUIDED_SETPOINT_NED:
      if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
      uint8_t flags = DL_GUIDED_SETPOINT_NED_flags(dl_buffer);
      float x = DL_GUIDED_SETPOINT_NED_x(dl_buffer);
      float y = DL_GUIDED_SETPOINT_NED_y(dl_buffer);
      float z = DL_GUIDED_SETPOINT_NED_z(dl_buffer);
      float yaw = DL_GUIDED_SETPOINT_NED_yaw(dl_buffer);
      switch (flags) {
        case 0x00:
        case 0x02:
          /* local NED position setpoints */
          autopilot_guided_goto_ned(x, y, z, yaw);
          break;
        case 0x01:
          /* local NED offset position setpoints */
          autopilot_guided_goto_ned_relative(x, y, z, yaw);
          break;
        case 0x03:
          /* body NED offset position setpoints */
          autopilot_guided_goto_body_relative(x, y, z, yaw);
          break;
        default:
          /* others not handled yet */
          break;
      }
      break;
    default:
      break;
  }
  /* Parse modules datalink */
  modules_parse_datalink(msg_id);
}