/// Control the mount (depends on the previously set mount configuration)
/// triggered by a MavLink packet.
void AP_Mount::control_msg(mavlink_message_t *msg)
{
__mavlink_mount_control_t packet;
mavlink_msg_mount_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
{
#if MNT_RETRACT_OPTION == ENABLED
case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
if (packet.save_position)
{
_retract_angles.save();
}
break;
#endif
case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
if (packet.save_position)
{
_neutral_angles.save();
}
break;
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
set_control_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
break;
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
{
Vector3f vec = _neutral_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
}
break;
#if MNT_GPSPOINT_OPTION == ENABLED
case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt
Location targetGPSLocation;
targetGPSLocation.lat = packet.input_a;
targetGPSLocation.lng = packet.input_b;
targetGPSLocation.alt = packet.input_c;
set_GPS_target_location(targetGPSLocation);
break;
#endif
case MAV_MOUNT_MODE_ENUM_END:
break;
default:
// do nothing
break;
}
}
/*
We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and
MAVLINK_MSG_ID_SCALED_PRESSUREs
*/
void Tracker::mavlink_snoop(const mavlink_message_t* msg)
{
// return immediately if sysid doesn't match our target sysid
if ((g.sysid_target != 0) && (g.sysid_target != msg->sysid)) {
return;
}
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavlink_check_target(msg);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracking_update_pressure(packet);
break;
}
}
}
/**
handle all types of log download requests from the GCS
*/
void GCS_MAVLINK::handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_list_t packet;
mavlink_msg_log_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
_log_listing = false;
_log_sending = false;
_log_num_logs = dataflash.get_num_logs();
if (_log_num_logs == 0) {
return;
}
int16_t last_log_num = dataflash.find_last_log();
_log_next_list_entry = packet.start;
_log_last_list_entry = packet.end;
if (_log_last_list_entry > last_log_num) {
_log_last_list_entry = last_log_num;
}
if (_log_next_list_entry < last_log_num + 1 - _log_num_logs) {
_log_next_list_entry = last_log_num + 1 - _log_num_logs;
}
_log_listing = true;
handle_log_send_listing(dataflash);
}
/*
handle a MISSION_COUNT mavlink packet
*/
void GCS_MAVLINK::handle_mission_count(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// start waypoint receiving
if (packet.count > mission.num_commands_max()) {
// send NAK
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_NO_SPACE);
return;
}
// new mission arriving, truncate mission to be the same length
mission.truncate(packet.count);
// set variables to help handle the expected receiving of commands from the GCS
waypoint_timelast_receive = hal.scheduler->millis(); // set time we last received commands to now
waypoint_receiving = true; // record that we expect to receive commands
waypoint_request_i = 0; // reset the next expected command number to zero
waypoint_request_last = packet.count; // record how many commands we expect to receive
waypoint_timelast_request = 0; // set time we last requested commands to zero
}
/*
handle a MISSION_WRITE_PARTIAL_LIST mavlink packet
*/
void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// start waypoint receiving
if ((unsigned)packet.start_index > mission.num_commands() ||
(unsigned)packet.end_index > mission.num_commands() ||
packet.end_index < packet.start_index) {
send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));
return;
}
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_timelast_request = 0;
waypoint_receiving = true;
waypoint_request_i = packet.start_index;
waypoint_request_last= packet.end_index;
}
/// decode MavLink that controls camera
void
AP_Camera::control_msg(mavlink_message_t* msg)
{
__mavlink_digicam_control_t packet;
mavlink_msg_digicam_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// This values may or not be used by APM (the shot is)
// They are bypassed as "echo" to a external specialized board
/*
* packet.command_id
* packet.extra_param
* packet.extra_value
* packet.focus_lock
* packet.session
* packet.shot
* packet.zoom_pos
* packet.zoom_step
*/
if (packet.shot)
{
trigger_pic();
}
}
/// decode MavLink that configures camera
void
AP_Camera::configure_msg(mavlink_message_t* msg)
{
__mavlink_digicam_configure_t packet;
mavlink_msg_digicam_configure_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// This values may or not be used by APM
// They are bypassed as "echo" to a external specialized board
/*
* packet.aperture
* packet.command_id
* packet.engine_cut_off
* packet.exposure_type
* packet.extra_param
* packet.extra_value
* packet.iso
* packet.mode
* packet.shutter_speed
*/
// echo the message to the ArduCam OSD camera control board
// for more info see: http://code.google.com/p/arducam-osd/
// TODO is it connected to MAVLINK_COMM_3 ?
mavlink_msg_digicam_configure_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.mode, packet.shutter_speed, packet.aperture, packet.iso, packet.exposure_type, packet.command_id, packet.engine_cut_off, packet.extra_param, packet.extra_value);
}
/// decode MavLink that controls camera
void
AP_Camera::control_msg(mavlink_message_t* msg)
{
__mavlink_digicam_control_t packet;
mavlink_msg_digicam_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// This values may or not be used by APM (the shot is)
// They are bypassed as "echo" to a external specialized board
/*
* packet.command_id
* packet.extra_param
* packet.extra_value
* packet.focus_lock
* packet.session
* packet.shot
* packet.zoom_pos
* packet.zoom_step
*/
if (packet.shot)
{
trigger_pic();
}
// echo the message to the ArduCam OSD camera control board
// for more info see: http://code.google.com/p/arducam-osd/
// TODO is it connected to MAVLINK_COMM_3 ?
mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value);
}
/// decode MavLink that controls camera
void
AP_Camera::control_msg(mavlink_message_t* msg)
{
__mavlink_digicam_control_t packet;
mavlink_msg_digicam_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// TODO do something with these values
/*
packet.command_id
packet.extra_param
packet.extra_value
packet.focus_lock
packet.session
packet.shot
packet.zoom_pos
packet.zoom_step
*/
if (packet.shot)
{
trigger_pic();
}
// echo the message to the ArduCam OSD camera control board
// for more info see: http://code.google.com/p/arducam-osd/
// TODO is it connected to MAVLINK_COMM_3 ?
mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value);
}
/// decode MavLink that configures camera
void
AP_Camera::configure_msg(mavlink_message_t* msg)
{
__mavlink_digicam_configure_t packet;
mavlink_msg_digicam_configure_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// TODO do something with these values
/*
packet.aperture
packet.command_id
packet.engine_cut_off
packet.exposure_type
packet.extra_param
packet.extra_value
packet.iso
packet.mode
packet.shutter_speed
*/
// echo the message to the ArduCam OSD camera control board
// for more info see: http://code.google.com/p/arducam-osd/
// TODO is it connected to MAVLINK_COMM_3 ?
mavlink_msg_digicam_configure_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.mode, packet.shutter_speed, packet.aperture, packet.iso, packet.exposure_type, packet.command_id, packet.engine_cut_off, packet.extra_param, packet.extra_value);
}
/*
We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and
MAVLINK_MSG_ID_SCALED_PRESSUREs
*/
void GCS_MAVLINK_Tracker::packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg)
{
// return immediately if sysid doesn't match our target sysid
if ((tracker.g.sysid_target != 0) && (tracker.g.sysid_target != msg.sysid)) {
return;
}
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavlink_check_target(msg);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
tracker.tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(&msg, &packet);
tracker.tracking_update_pressure(packet);
break;
}
}
GCS_MAVLINK::packetReceived(status, msg);
}
/// Return mount status information (depends on the previously set mount configuration)
/// triggered by a MavLink packet.
void AP_Mount::status_msg(mavlink_message_t *msg)
{
__mavlink_mount_status_t packet;
mavlink_msg_mount_status_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
{
case MAV_MOUNT_MODE_RETRACT: // safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
case MAV_MOUNT_MODE_NEUTRAL: // neutral position (Roll,Pitch,Yaw) from EEPROM
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
case MAV_MOUNT_MODE_RC_TARGETING: // neutral position and start RC Roll,Pitch,Yaw control with stabilization
packet.pointing_b = _roll_angle*100; // degrees*100
packet.pointing_a = _tilt_angle*100; // degrees*100
packet.pointing_c = _pan_angle*100; // degrees*100
break;
#if MNT_GPSPOINT_OPTION == ENABLED
case MAV_MOUNT_MODE_GPS_POINT: // neutral position and start to point to Lat,Lon,Alt
packet.pointing_a = _target_GPS_location.lat; // latitude
packet.pointing_b = _target_GPS_location.lng; // longitude
packet.pointing_c = _target_GPS_location.alt; // altitude
break;
#endif
case MAV_MOUNT_MODE_ENUM_END:
break;
}
// status reply
// TODO: is COMM_3 correct ?
mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component,
packet.pointing_a, packet.pointing_b, packet.pointing_c);
}
/*
handle a MISSION_REQUEST mavlink packet
*/
void GCS_MAVLINK::handle_mission_request(AP_Mission &mission, mavlink_message_t *msg)
{
AP_Mission::Mission_Command cmd;
// decode
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// retrieve mission from eeprom
if (!mission.read_cmd_from_storage(packet.seq, cmd)) {
goto mission_item_send_failed;
}
// convert mission command to mavlink mission item packet
mavlink_mission_item_t ret_packet;
memset(&ret_packet, 0, sizeof(ret_packet));
if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) {
goto mission_item_send_failed;
}
// set packet's current field to 1 if this is the command being executed
if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {
ret_packet.current = 1;
} else {
ret_packet.current = 0;
}
// set auto continue to 1
ret_packet.autocontinue = 1; // 1 (true), 0 (false)
/*
avoid the _send() function to save memory on APM2, as it avoids
the stack usage of the _send() function by using the already
declared ret_packet above
*/
ret_packet.target_system = msg->sysid;
ret_packet.target_component = msg->compid;
ret_packet.seq = packet.seq;
ret_packet.command = cmd.id;
_mav_finalize_message_chan_send(chan,
MAVLINK_MSG_ID_MISSION_ITEM,
(const char *)&ret_packet,
MAVLINK_MSG_ID_MISSION_ITEM_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_CRC);
return;
mission_item_send_failed:
// send failure message
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);
}
/// Change the configuration of the mount
/// triggered by a MavLink packet.
void AP_Mount::configure_msg(mavlink_message_t* msg)
{
__mavlink_mount_configure_t packet;
mavlink_msg_mount_configure_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
_stab_roll = packet.stab_roll;
_stab_tilt = packet.stab_pitch;
_stab_pan = packet.stab_yaw;
}
/*
handle a MISSION_SET_CURRENT mavlink packet
*/
void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_set_current_t packet;
mavlink_msg_mission_set_current_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// set current command
if (mission.set_current_cmd(packet.seq)) {
mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
}
}
static inline void MissionAck(mavlink_message_t* handle_msg)
{
mavlink_mission_ack_t packet;
//send_text((uint8_t*)"waypoint ack\r\n");
DPRINT("mission ack\r\n");
// decode
mavlink_msg_mission_ack_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
// parse for error - although we do nothing about an error.
//uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send
mavlink_flags.mavlink_sending_waypoints = false;
mavlink_waypoint_timeout = 0;
}
/*
handle a MISSION_REQUEST_LIST mavlink packet
*/
void GCS_MAVLINK::handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_request_list_t packet;
mavlink_msg_mission_request_list_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// reply with number of commands in the mission. The GCS will then request each command separately
mavlink_msg_mission_count_send(chan,msg->sysid, msg->compid, mission.num_commands());
// set variables to help handle the expected sending of commands to the GCS
waypoint_receiving = false; // record that we are sending commands (i.e. not receiving)
waypoint_dest_sysid = msg->sysid; // record system id of GCS who has requested the commands
waypoint_dest_compid = msg->compid; // record component id of GCS who has requested the commands
}
static inline void MissionRequestList(mavlink_message_t* handle_msg)
{
mavlink_mission_request_list_t packet;
// BULDING
// DPRINT("mission request list\r\n");
// decode
mavlink_msg_mission_request_list_decode(handle_msg, &packet);
DPRINT("mission request list: target_system %u, target_component %u\r\n", packet.target_system, packet.target_component);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
mavlink_waypoint_timeout = MAVLINK_WAYPOINT_TIMEOUT;
mavlink_flags.mavlink_sending_waypoints = true;
mavlink_flags.mavlink_receiving_waypoints = false;
mavlink_waypoint_dest_sysid = handle_msg->sysid;
mavlink_waypoint_dest_compid = handle_msg->compid;
// Start sending waypoints
mavlink_flags.mavlink_send_waypoint_count = 1;
DPRINT("mission request list: sysid %u compid %u\r\n", handle_msg->sysid, handle_msg->compid);
}
/*
handle a MISSION_CLEAR_ALL mavlink packet
*/
void GCS_MAVLINK::handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_clear_all_t packet;
mavlink_msg_mission_clear_all_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// clear all waypoints
if (mission.clear()) {
// send ack
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_RESULT_ACCEPTED);
}else{
// send nack
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, 1);
}
}
void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg)
{
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
#if CONFIG_HAL_BOARD != HAL_BOARD_APM1 && CONFIG_HAL_BOARD != HAL_BOARD_APM2
// send system ID if we can
char sysid[40];
if (hal.util->get_system_id(sysid)) {
send_text(SEVERITY_LOW, sysid);
}
#endif
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
}
/**
handle request for log data
*/
void GCS_MAVLINK::handle_log_request_data(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_data_t packet;
mavlink_msg_log_request_data_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
_log_listing = false;
if (!_log_sending || _log_num_data != packet.id) {
_log_sending = false;
uint16_t num_logs = dataflash.get_num_logs();
int16_t last_log_num = dataflash.find_last_log();
if (packet.id > last_log_num || packet.id < last_log_num + 1 - num_logs) {
return;
}
uint32_t time_utc, size;
dataflash.get_log_info(packet.id, size, time_utc);
_log_num_data = packet.id;
_log_data_size = size;
uint16_t end;
dataflash.get_log_boundaries(packet.id, _log_data_page, end);
}
_log_data_offset = packet.ofs;
if (_log_data_offset >= _log_data_size) {
_log_data_remaining = 0;
} else {
_log_data_remaining = _log_data_size - _log_data_offset;
}
if (_log_data_remaining > packet.count) {
_log_data_remaining = packet.count;
}
_log_sending = true;
handle_log_send(dataflash);
}
static inline void MissionSetCurrent(mavlink_message_t* handle_msg)
{
mavlink_mission_set_current_t packet;
//send_text((uint8_t*)"waypoint set current\r\n");
DPRINT("mission set current\r\n");
// decode
mavlink_msg_mission_set_current_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
DPRINT("mission set current: %u\r\n", packet.seq);
// set current waypoint
set(PARAM_WP_INDEX, packet.seq);
set_waypoint(packet.seq);
{
//Location temp; // XXX this is gross
//temp = get_wp_with_index(packet.seq);
//set_next_WP(&temp);
}
mavlink_msg_mission_current_send(MAVLINK_COMM_0, get(PARAM_WP_INDEX));
}
/// decode MavLink that configures camera
void
AP_Camera::configure_msg(mavlink_message_t* msg)
{
__mavlink_digicam_configure_t packet;
mavlink_msg_digicam_configure_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// This values may or not be used by APM
// They are bypassed as "echo" to a external specialized board
/*
* packet.aperture
* packet.command_id
* packet.engine_cut_off
* packet.exposure_type
* packet.extra_param
* packet.extra_value
* packet.iso
* packet.mode
* packet.shutter_speed
*/
}
static inline void MissionClearAll(mavlink_message_t* handle_msg)
{
mavlink_mission_clear_all_t packet;
uint8_t type = 0; // ok (0), error(1)
int16_t i;
//send_text((uint8_t*)"waypoint clear all\r\n");
DPRINT("mission clear all\r\n");
// decode
mavlink_msg_mission_clear_all_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
// clear all waypoints
set(PARAM_WP_TOTAL, 0);
clear_flightplan();
// send acknowledgement 3 times to makes sure it is received
for (i = 0; i < 3; i++)
{
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, handle_msg->sysid, handle_msg->compid, type);
}
}
static inline void MissionCount(mavlink_message_t* handle_msg)
{
mavlink_mission_count_t packet;
//send_text((uint8_t*)"waypoint count\r\n");
DPRINT("mission count\r\n");
// decode
mavlink_msg_mission_count_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
DPRINT("mission count: %u\r\n", packet.count);
// start waypoint receiving
set(PARAM_WP_TOTAL, packet.count);
if (get(PARAM_WP_TOTAL) > MAX_WAYPOINTS)
set(PARAM_WP_TOTAL, MAX_WAYPOINTS);
//mavlink_flags.waypoint_timelast_receive = millis();
mavlink_waypoint_timeout = MAVLINK_WAYPOINT_TIMEOUT;
mavlink_flags.mavlink_receiving_waypoints = true;
mavlink_flags.mavlink_sending_waypoints = false;
mavlink_flags.mavlink_request_specific_waypoint = 1;
waypoint_request_i = 0;
}
void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg)
{
mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
enum ap_var_type p_type;
AP_Param *vp;
char param_name[AP_MAX_NAME_SIZE+1];
if (packet.param_index != -1) {
AP_Param::ParamToken token;
vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);
if (vp == NULL) {
return;
}
vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);
param_name[AP_MAX_NAME_SIZE] = 0;
} else {
strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
param_name[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::find(param_name, &p_type);
if (vp == NULL) {
return;
}
}
float value = vp->cast_to_float(p_type);
mavlink_msg_param_value_send_buf(
msg,
chan,
param_name,
value,
mav_var_type(p_type),
_count_parameters(),
packet.param_index);
}
/*
handle an incoming mission item
*/
void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
{
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
struct AP_Mission::Mission_Command cmd = {};
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
result = MAV_MISSION_INVALID;
goto mission_ack;
}
if (packet.current == 2) {
// current = 2 is a flag to tell us this is a "guided mode"
// waypoint and not for the mission
handle_guided_request(cmd);
// verify we received the command
result = 0;
goto mission_ack;
}
if (packet.current == 3) {
//current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
handle_change_alt_request(cmd);
// verify we recevied the command
result = 0;
goto mission_ack;
}
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_ack;
}
// if command index is within the existing list, replace the command
if (packet.seq < mission.num_commands()) {
if (mission.replace_cmd(packet.seq,cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if command is at the end of command list, add the command
} else if (packet.seq == mission.num_commands()) {
if (mission.add_cmd(cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if beyond the end of the command list, return an error
} else {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// update waypoint receiving state machine
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_request_i++;
if (waypoint_request_i >= waypoint_request_last) {
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
MAV_MISSION_ACCEPTED);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
} else {
waypoint_timelast_request = hal.scheduler->millis();
// if we have enough space, then send the next WP immediately
if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
queued_waypoint_send();
} else {
send_message(MSG_NEXT_WAYPOINT);
}
}
return;
mission_ack:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result);
}
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
AP_Param *vp;
enum ap_var_type var_type;
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// set parameter
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find the requested parameter
vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
float rounding_addition = 0.01;
// handle variables with standard type IDs
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter
return;
}
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
if (DataFlash != NULL) {
DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
}
}
}
/*
handle a request to change stream rate. Note that copter passes in
save==false, as sending mavlink messages on copter on APM2 costs
enough that it can cause flight issues, so we don't want the save to
happen when the user connects the ground station.
*/
void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg, bool save)
{
mavlink_request_data_stream_t packet;
mavlink_msg_request_data_stream_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
int16_t freq = 0; // packet frequency
if (packet.start_stop == 0)
freq = 0; // stop sending
else if (packet.start_stop == 1)
freq = packet.req_message_rate; // start sending
else
return;
AP_Int16 *rate = NULL;
switch (packet.req_stream_id) {
case MAV_DATA_STREAM_ALL:
// note that we don't set STREAM_PARAMS - that is internal only
for (uint8_t i=0; i<STREAM_PARAMS; i++) {
if (save) {
streamRates[i].set_and_save_ifchanged(freq);
} else {
streamRates[i].set(freq);
}
}
break;
case MAV_DATA_STREAM_RAW_SENSORS:
rate = &streamRates[STREAM_RAW_SENSORS];
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
rate = &streamRates[STREAM_EXTENDED_STATUS];
break;
case MAV_DATA_STREAM_RC_CHANNELS:
rate = &streamRates[STREAM_RC_CHANNELS];
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
rate = &streamRates[STREAM_RAW_CONTROLLER];
break;
case MAV_DATA_STREAM_POSITION:
rate = &streamRates[STREAM_POSITION];
break;
case MAV_DATA_STREAM_EXTRA1:
rate = &streamRates[STREAM_EXTRA1];
break;
case MAV_DATA_STREAM_EXTRA2:
rate = &streamRates[STREAM_EXTRA2];
break;
case MAV_DATA_STREAM_EXTRA3:
rate = &streamRates[STREAM_EXTRA3];
break;
}
if (rate != NULL) {
if (save) {
rate->set_and_save_ifchanged(freq);
} else {
rate->set(freq);
}
}
}
boolean MAVFlexiFunctionsHandleMessage(mavlink_message_t* handle_msg)
{
switch (handle_msg->msgid)
{
#if (USE_FLEXIFUNCTION_MIXING == 1)
case MAVLINK_MSG_ID_FLEXIFUNCTION_SET:
// Do nothing with this funciton since it is obsolete
// Must keep function defined to activate flexifunction mavlink libraries
break;
case MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION:
{
mavlink_flexifunction_buffer_function_t packet;
mavlink_msg_flexifunction_buffer_function_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) break;
// can't respond if busy doing something
if (flexiFunctionState != FLEXIFUNCTION_WAITING) break;
flexiFunction_write_buffer_function(&packet.data[0],
packet.func_index,
packet.data_address,
packet.data_size,
packet.func_count);
break;
}
case MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY:
{
mavlink_flexifunction_directory_t packet;
mavlink_msg_flexifunction_directory_decode(handle_msg, &packet);
// can't respond if busy doing something
if (flexiFunctionState != FLEXIFUNCTION_WAITING) break;
flexiFunction_write_directory(packet.directory_type, packet.start_index, packet.count, packet.directory_data);
break;
}
case MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND:
{
mavlink_flexifunction_command_t packet;
mavlink_msg_flexifunction_command_decode(handle_msg, &packet);
// can't respond if busy doing something
if (flexiFunctionState != FLEXIFUNCTION_WAITING) break;
switch (packet.command_type)
{
case FLEXIFUNCTION_COMMAND_COMMIT_BUFFER:
flexiFunctionState = FLEXIFUNCTION_COMMIT_BUFFER;
break;
case FLEXIFUNCTION_COMMAND_WRITE_NVMEMORY:
flexiFunctionState = FLEXIFUNCTION_WRITE_NVMEMORY;
break;
}
break;
}
#endif // (USE_FLEXIFUNCTION_MIXING == 1)
default:
return false;
}
return true;
}
