Exemplo n.º 1
0
void dl_parse_msg( void ) {
    uint8_t msg_id = IdOfMsg(dl_buffer);
    if (msg_id == DL_SET_ACTUATOR) {
        uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
        uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
        LED_TOGGLE(2);
        if (servo_no < SERVOS_NB)
            SetServo(servo_no, servo_value);
    }
#ifdef DlSetting
    else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
        uint8_t i = DL_SETTING_index(dl_buffer);
        float val = DL_SETTING_value(dl_buffer);
        DlSetting(i, val);
        LED_TOGGLE(2);
        for (int j=0 ; j<8 ; j++) {
            SetServo(j,actuators[j]);
        }
        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
    } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
        uint8_t i = DL_GET_SETTING_index(dl_buffer);
        float val = settings_get_value(i);
        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
    }
#endif
}
Exemplo n.º 2
0
static void
update_service(service_instance_t *si)
{
  if(si->si_service == NULL &&
     si->si_setting_enabled &&
     si->si_setting_title &&
     si->si_setting_type) {

    si->si_service = service_create(NULL, si->si_url, NULL, NULL,
				    si->si_probe, 0);
    prop_link(settings_get_value(si->si_setting_title), 
	      prop_create(si->si_service->s_root, "title"));
    prop_link(settings_get_value(si->si_setting_type), 
	      prop_create(si->si_service->s_root, "type"));
    prop_link(settings_get_value(si->si_setting_enabled), 
	      prop_create(si->si_service->s_root, "enabled"));
  }
}
Exemplo n.º 3
0
/**
 * Send the parameters
 */
static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
{
  if (mavlink_params_idx >= NB_SETTING) {
    return;
  }

  mavlink_msg_param_value_send(MAVLINK_COMM_0,
                               mavlink_param_names[mavlink_params_idx],
                               settings_get_value(mavlink_params_idx),
                               MAV_PARAM_TYPE_REAL32,
                               NB_SETTING,
                               mavlink_params_idx);
  MAVLinkSendMessage();

  mavlink_params_idx++;
}
Exemplo n.º 4
0
/**
 * Send the parameters
 */
static inline void mavlink_send_params(void)
{
  if (mavlink_params_idx >= NB_SETTING) {
    return;
  }

  mavlink_msg_param_value_send(MAVLINK_COMM_0,
                               mavlink_param_names[mavlink_params_idx],
                               settings_get_value(mavlink_params_idx),
                               MAV_PARAM_TYPE_REAL32,
                               NB_SETTING,
                               mavlink_params_idx);
  MAVLinkSendMessage();

  mavlink_params_idx++;
}
Exemplo n.º 5
0
void dl_parse_msg(void) {

  datalink_time = 0;

  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;

/* this case for DL_HOVER_POINT_CHANGE_ECEF is added by Yi and Edward */
/*********************************************************************/
  case DL_HOVER_POINT_CHANGE_ECEF:
    {  
      uint8_t ac_id = DL_HOVER_POINT_CHANGE_ECEF_ac_id(dl_buffer);
      if(ac_id != AC_ID) break;
      uint8_t external_gps_fixed = DL_HOVER_POINT_CHANGE_ECEF_external_gps_fix(dl_buffer);

      /** getting the ecef coordinate from external gps **/
      struct EcefCoor_i external_gps_ecef_pos;
      external_gps_ecef_pos.x = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x(dl_buffer);//DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x() is defined in dl_protocol.h
      external_gps_ecef_pos.y = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_y(dl_buffer);
      external_gps_ecef_pos.z = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_z(dl_buffer);

      DOWNLINK_SEND_HOVER_POINT_CHANGE_ECEF(DefaultChannel, DefaultDevice, &ac_id, &external_gps_fixed, &(external_gps_ecef_pos.x), &(external_gps_ecef_pos.y), &(external_gps_ecef_pos.z));

      if(external_gps_fixed == 0x03)//case for 3D_Fix.
      {
        if(!ins_ltp_initialised || guidance_h_mode != GUIDANCE_H_MODE_HOVER) break;//ins_ltp_initialised defined in ins_int.h 
                                                                                   //no HMOE reference
                                                                                   //not in the HOVER_Z_HOLD mode
		                                                                   //not in flight(looks like no needed)	  
	struct NedCoor_i external_gps_ned_pos;
	memset((void *) &external_gps_ned_pos,0,sizeof(struct NedCoor_i));//just in case, clear the memory.
		  
	/** convert the ecef coordinate into ned(ltp) coordinate **/
	ned_of_ecef_point_i(&external_gps_ned_pos,&ins_ltp_def,&external_gps_ecef_pos);
		  
		  
	/** change the current setpoint with external_gps_ned_pos coordinate **/
	struct Int32Vect2 external_gps_h_pos;
	//cm to m and store in external_gps_h_pos
	//check function ins_update_gps() for reference.
        INT32_VECT2_SCALE_2(external_gps_h_pos,external_gps_ned_pos,INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
	//subsitute the guidance_h_pos_sp
	VECT2_COPY(guidance_h_pos_sp,external_gps_h_pos);
	/*
	 * the above code is used for congruity, but waste space and time.
	 * simple using
	 * 
	 * guidance_h_pos_sp.x = external_gps_ned_pos.x;
	 * guidance_h_pos_sp.y = external_gps_ned_pox.y;
	 * 
	 * can achieve the same effect.
	*/
      }
      
      else break;//hover pos_sp no change
    } 
/**------------------------------------------------------------------*/


#if defined 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;
      uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
      struct LlaCoor_i lla;
      struct EnuCoor_i enu;
      lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
      lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
      /* WP_alt is in cm, lla.alt in mm */
      lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
      enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
      enu.x = POS_BFP_OF_REAL(enu.x)/100;
      enu.y = POS_BFP_OF_REAL(enu.y)/100;
      enu.z = POS_BFP_OF_REAL(enu.z)/100;
      VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
      DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
    }
    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 :
#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
  default:
    break;
  }
  /* Parse modules datalink */
  modules_parse_datalink(msg_id);
}
Exemplo n.º 6
0
void mavlink_common_message_handler(const mavlink_message_t *msg)
{
  switch (msg->msgid) {
    case MAVLINK_MSG_ID_HEARTBEAT:
      break;

      /* When requesting data streams say we can't send them */
    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
      mavlink_request_data_stream_t cmd;
      mavlink_msg_request_data_stream_decode(msg, &cmd);

      mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
      MAVLinkSendMessage();
      break;
    }

    /* Override channels with RC */
    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
      mavlink_rc_channels_override_t cmd;
      mavlink_msg_rc_channels_override_decode(msg, &cmd);
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
      uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
      int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
      int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
      int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
      parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw);
      //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
      // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
#endif
      break;
    }

    /* When a request is made of the parameters list */
    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
      mavlink_params_idx = 0;
      break;
    }

    /* When a request os made for a specific parameter */
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
      mavlink_param_request_read_t cmd;
      mavlink_msg_param_request_read_decode(msg, &cmd);

      // First check param_index and search for the ID if needed
      if (cmd.param_index == -1) {
        cmd.param_index = settings_idx_from_param_id(cmd.param_id);
      }

      // Send message only if the param_index was found (Coverity Scan)
      if (cmd.param_index > -1) {
        mavlink_msg_param_value_send(MAVLINK_COMM_0,
                                     mavlink_param_names[cmd.param_index],
                                     settings_get_value(cmd.param_index),
                                     MAV_PARAM_TYPE_REAL32,
                                     NB_SETTING,
                                     cmd.param_index);
        MAVLinkSendMessage();
      }
      break;
    }

    case MAVLINK_MSG_ID_PARAM_SET: {
      mavlink_param_set_t set;
      mavlink_msg_param_set_decode(msg, &set);

      // Check if this message is for this system
      if ((uint8_t) set.target_system == AC_ID) {
        int16_t idx = settings_idx_from_param_id(set.param_id);

        // setting found
        if (idx >= 0) {
          // Only write if new value is NOT "not-a-number"
          // AND is NOT infinity
          if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
              !isnan(set.param_value) && !isinf(set.param_value)) {
            DlSetting(idx, set.param_value);
            // Report back new value
            mavlink_msg_param_value_send(MAVLINK_COMM_0,
                                         mavlink_param_names[idx],
                                         settings_get_value(idx),
                                         MAV_PARAM_TYPE_REAL32,
                                         NB_SETTING,
                                         idx);
            MAVLinkSendMessage();
          }
        }
      }
    }
    break;

#ifndef AP
    /* only for rotorcraft */
    case MAVLINK_MSG_ID_COMMAND_LONG: {
      mavlink_command_long_t cmd;
      mavlink_msg_command_long_decode(msg, &cmd);
      // Check if this message is for this system
      if ((uint8_t) cmd.target_system == AC_ID) {
        uint8_t result = MAV_RESULT_UNSUPPORTED;
        switch (cmd.command) {
          case MAV_CMD_NAV_GUIDED_ENABLE:
            MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
            result = MAV_RESULT_FAILED;
            if (cmd.param1 > 0.5) {
              autopilot_set_mode(AP_MODE_GUIDED);
              if (autopilot_mode == AP_MODE_GUIDED) {
                result = MAV_RESULT_ACCEPTED;
              }
            }
            else {
              // turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
            }
            break;

          case MAV_CMD_COMPONENT_ARM_DISARM:
            /* supposed to use this command to arm or SET_MODE?? */
            MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
            result = MAV_RESULT_FAILED;
            if (cmd.param1 > 0.5) {
              autopilot_set_motors_on(TRUE);
              if (autopilot_motors_on)
                result = MAV_RESULT_ACCEPTED;
            }
            else {
              autopilot_set_motors_on(FALSE);
              if (!autopilot_motors_on)
                result = MAV_RESULT_ACCEPTED;
            }
            break;

          default:
            break;
        }
        // confirm command with result
        mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result);
        MAVLinkSendMessage();
      }
      break;
    }

    case MAVLINK_MSG_ID_SET_MODE: {
      mavlink_set_mode_t mode;
      mavlink_msg_set_mode_decode(msg, &mode);
      if (mode.target_system == AC_ID) {
        MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
        if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
          autopilot_set_motors_on(TRUE);
        }
        else {
          autopilot_set_motors_on(FALSE);
        }
        if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
          autopilot_set_mode(AP_MODE_GUIDED);
        }
        else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
          autopilot_set_mode(AP_MODE_NAV);
        }
      }
      break;
    }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
      mavlink_set_position_target_local_ned_t target;
      mavlink_msg_set_position_target_local_ned_decode(msg, &target);
      // Check if this message is for this system
      if (target.target_system == AC_ID) {
        MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
        /* if position and yaw bits are not set to ignored, use only position for now */
        if (!(target.type_mask & 0b1110000000100000)) {
          switch (target.coordinate_frame) {
            case MAV_FRAME_LOCAL_NED:
              MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
              autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw);
              break;
            case MAV_FRAME_LOCAL_OFFSET_NED:
              MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
              autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw);
              break;
            case MAV_FRAME_BODY_OFFSET_NED:
              MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
              autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw);
              break;
            default:
              break;
          }
        }
        else if (!(target.type_mask & 0b0001110000100000)) {
          /* position is set to ignore, but velocity not */
          switch (target.coordinate_frame) {
            case MAV_FRAME_LOCAL_NED:
              MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n");
              autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw);
              break;
            default:
              break;
          }
        }
      }
      break;
    }
#endif

    default:
      //Do nothing
      MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
      break;
  }
}
Exemplo n.º 7
0
void dl_parse_msg(void)
{

  datalink_time = 0;

  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;

#if defined 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 defined GPS_DATALINK
    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
    default:
      break;
  }
  /* Parse modules datalink */
  modules_parse_datalink(msg_id);
}
Exemplo n.º 8
0
void mavlink_common_message_handler(const mavlink_message_t *msg)
{
  switch (msg->msgid) {
    case MAVLINK_MSG_ID_HEARTBEAT:
      break;

      /* When requesting data streams say we can't send them */
    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
      mavlink_request_data_stream_t cmd;
      mavlink_msg_request_data_stream_decode(msg, &cmd);

      mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
      MAVLinkSendMessage();
      break;
    }

      /* Override channels with RC */
    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
      mavlink_rc_channels_override_t cmd;
      mavlink_msg_rc_channels_override_decode(msg, &cmd);
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
      uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
      int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
      int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
      int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
      parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw);
      //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
      // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
#endif
      break;
    }

      /* When a request is made of the parameters list */
    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
      mavlink_params_idx = 0;
      break;
    }

      /* When a request os made for a specific parameter */
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
      mavlink_param_request_read_t cmd;
      mavlink_msg_param_request_read_decode(msg, &cmd);

      // First check param_index and search for the ID if needed
      if (cmd.param_index == -1) {
        cmd.param_index = settings_idx_from_param_id(cmd.param_id);
      }

      mavlink_msg_param_value_send(MAVLINK_COMM_0,
                                   mavlink_param_names[cmd.param_index],
                                   settings_get_value(cmd.param_index),
                                   MAV_PARAM_TYPE_REAL32,
                                   NB_SETTING,
                                   cmd.param_index);
      MAVLinkSendMessage();

      break;
    }

    case MAVLINK_MSG_ID_PARAM_SET: {
      mavlink_param_set_t set;
      mavlink_msg_param_set_decode(msg, &set);

      // Check if this message is for this system
      if ((uint8_t) set.target_system == AC_ID) {
        int16_t idx = settings_idx_from_param_id(set.param_id);

        // setting found
        if (idx >= 0) {
          // Only write if new value is NOT "not-a-number"
          // AND is NOT infinity
          if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
              !isnan(set.param_value) && !isinf(set.param_value)) {
            DlSetting(idx, set.param_value);
            // Report back new value
            mavlink_msg_param_value_send(MAVLINK_COMM_0,
                                         mavlink_param_names[idx],
                                         settings_get_value(idx),
                                         MAV_PARAM_TYPE_REAL32,
                                         NB_SETTING,
                                         idx);
            MAVLinkSendMessage();
          }
        }
      }
    }
      break;

    default:
      //Do nothing
      //MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
      break;
  }
}
Exemplo n.º 9
0
void dl_parse_msg(void) {
  datalink_time = 0;
  uint8_t msg_id = IdOfMsg(dl_buffer);
#if 0 // not ready yet
  uint8_t sender_id = SenderIdOfMsg(dl_buffer);

  /* parse telemetry messages coming from other AC */
  if (sender_id != 0) {
    switch (msg_id) {
#ifdef TCAS
      case DL_TCAS_RA:
        {
          if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
            uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
            tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
          }
        }
#endif
    }
    return;
  }
#endif

  if (msg_id == DL_PING) {
    DOWNLINK_SEND_PONG(DefaultChannel);
  } else
#ifdef TRAFFIC_INFO
  if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
    uint8_t id = DL_ACINFO_ac_id(dl_buffer);
    float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
    float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
    float a = MOfCm(DL_ACINFO_alt(dl_buffer));
    float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
    float s = MOfCm(DL_ACINFO_speed(dl_buffer));
    float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
    uint32_t t = DL_ACINFO_itow(dl_buffer);
    SetAcInfo(id, ux, uy, c, a, s, cl, t);
  } else
#endif
#ifdef NAV
  if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
    uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
    float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));

    /* Computes from (lat, long) in the referenced UTM zone */
    float lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
    float lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
    latlong_utm_of(lat, lon, nav_utm_zone0);
    nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a);

    /* Waypoint range is limited. Computes the UTM pos back from the relative
       coordinates */
    latlong_utm_x = waypoints[wp_id].x + nav_utm_east0;
    latlong_utm_y = waypoints[wp_id].y + nav_utm_north0;
    DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0);
  } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
    nav_goto_block(DL_BLOCK_block_id(dl_buffer));
    SEND_NAVIGATION(DefaultChannel);
  } else
#endif /** NAV */
#ifdef WIND_INFO
  if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
    wind_east = DL_WIND_INFO_east(dl_buffer);
    wind_north = DL_WIND_INFO_north(dl_buffer);
#ifndef USE_AIRSPEED
    estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
#endif
#ifdef WIND_INFO_RET
    DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed);
#endif
  } else
#endif /** WIND_INFO */

#ifdef HITL
  /** Infrared and GPS sensors are replaced by messages on the datalink */
  if (msg_id == DL_HITL_INFRARED) {
    /** This code simulates infrared.c:ir_update() */
    ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
    ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
    ir_top = DL_HITL_INFRARED_top(dl_buffer);
  } else if (msg_id == DL_HITL_UBX) {
    /** This code simulates gps_ubx.c:parse_ubx() */
    if (gps_msg_received) {
      gps_nb_ovrn++;
    } else {
      ubx_class = DL_HITL_UBX_class(dl_buffer);
      ubx_id = DL_HITL_UBX_id(dl_buffer);
      uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
      uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
      memcpy(ubx_msg_buf, ubx_payload, l);
      gps_msg_received = TRUE;
    }
  } else
#endif
#ifdef DlSetting
  if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
    uint8_t i = DL_SETTING_index(dl_buffer);
    float val = DL_SETTING_value(dl_buffer);
    DlSetting(i, val);
    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
    uint8_t i = DL_GET_SETTING_index(dl_buffer);
    float val = settings_get_value(i);
    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
  } else
#endif /** Else there is no dl_settings section in the flight plan */
#ifdef USE_JOYSTICK
    if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
      JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
			    DL_JOYSTICK_RAW_pitch(dl_buffer),
			    DL_JOYSTICK_RAW_throttle(dl_buffer));
    } else
#endif // USE_JOYSTICK
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
    if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
LED_TOGGLE(3);
      parse_rc_datalink(
          DL_RC_3CH_throttle_mode(dl_buffer),
          DL_RC_3CH_roll(dl_buffer),
          DL_RC_3CH_pitch(dl_buffer));
    } else
#endif // RC_DATALINK
  { /* Last else */
    /* Parse modules datalink */
    modules_parse_datalink(msg_id);
  }
}
Exemplo n.º 10
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.º 11
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);
}