Exemplo n.º 1
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.º 2
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;
  }
}