From 81d137ad6227b5fa9316ef13d5d515df5fa85371 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 14 Nov 2015 23:05:35 +0100 Subject: [PATCH 01/12] [modules] mavlink: add some missing MAVLinkSendMessage() --- sw/airborne/modules/datalink/mavlink.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index c8a6e62864..5945a8ea06 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -484,6 +484,7 @@ static inline void mavlink_send_autopilot_version(void) 0, //uint16_t product_id, sha //uint64_t uid ); + MAVLinkSendMessage(); } static inline void mavlink_send_attitude_quaternion(void) @@ -497,6 +498,7 @@ static inline void mavlink_send_attitude_quaternion(void) stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r); + MAVLinkSendMessage(); } #if USE_GPS @@ -521,6 +523,7 @@ static inline void mavlink_send_gps_raw_int(void) gps.gspeed, course, gps.num_sv); + MAVLinkSendMessage(); #endif } @@ -562,6 +565,7 @@ static inline void mavlink_send_rc_channels(void) UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, 255); #endif + MAVLinkSendMessage(); } #include "subsystems/electrical.h" @@ -581,4 +585,5 @@ static inline void mavlink_send_battery_status(void) electrical.consumed, electrical.energy, // check scaling -1); // remaining percentage not estimated + MAVLinkSendMessage(); } From 9916d6d218e554b6958c334bcf0d202998e0ec2f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 14 Nov 2015 23:06:23 +0100 Subject: [PATCH 02/12] [modules] mavlink: send GPS_STATUS and VFR_HUD --- sw/airborne/modules/datalink/mavlink.c | 72 ++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 5945a8ea06..a71980a5d2 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -77,6 +77,8 @@ static inline void mavlink_send_gps_raw_int(void); static inline void mavlink_send_rc_channels(void); static inline void mavlink_send_battery_status(void); static inline void mavlink_send_gps_global_origin(void); +static inline void mavlink_send_gps_status(void); +static inline void mavlink_send_vfr_hud(void); /// TODO: FIXME @@ -112,6 +114,8 @@ void mavlink_periodic(void) RunOnceEvery(21, mavlink_send_battery_status()); RunOnceEvery(32, mavlink_send_autopilot_version()); RunOnceEvery(33, mavlink_send_gps_global_origin()); + RunOnceEvery(10, mavlink_send_gps_status()); + RunOnceEvery(10, mavlink_send_vfr_hud()); } static int16_t settings_idx_from_param_id(char *param_id) @@ -527,6 +531,48 @@ static inline void mavlink_send_gps_raw_int(void) #endif } +/** + * Send gps status. + * The GPS status message consists of: + * - satellites visible + * - satellite pnr[] (ignored) + * - satellite used[] (ignored) + * - satellite elevation[] (ignored) + * - satellite azimuth[] (ignored) + * - satellite snr[] (ignored) + */ +static inline void mavlink_send_gps_status(void) +{ +#if USE_GPS + uint8_t nb_sats = Min(GPS_NB_CHANNELS, 20); + uint8_t prn[20]; + uint8_t used[20]; + uint8_t elevation[20]; + uint8_t azimuth[20]; + uint8_t snr[20]; + for (uint8_t i=0; i < nb_sats; i++) { + prn[i] = gps.svinfos[i].svid; + // set sat as used if cno > 0, not quite true though + if (gps.svinfos[i].cno > 0) { + used[i] = 1; + } + elevation[i] = gps.svinfos[i].elev; + // convert azimuth to unsigned representation: 0: 0 deg, 255: 360 deg. + uint8_t azim; + if (gps.svinfos[i].azim < 0) { + azim = (float)(-gps.svinfos[i].azim + 180) / 360 * 255; + } + else { + azim = (float)gps.svinfos[i].azim / 360 * 255; + } + azimuth[i] = azim; + } + mavlink_msg_gps_status_send(MAVLINK_COMM_0, + gps.num_sv, prn, used, elevation, azimuth, snr); + MAVLinkSendMessage(); +#endif +} + #if defined RADIO_CONTROL #include "subsystems/radio_control.h" // since they really want PPM values, use a hack to check if are using ppm subsystem @@ -587,3 +633,29 @@ static inline void mavlink_send_battery_status(void) -1); // remaining percentage not estimated MAVLinkSendMessage(); } + +#include "subsystems/commands.h" +/** + * Send Metrics typically displayed on a HUD for fixed wing aircraft. + */ +static inline void mavlink_send_vfr_hud(void) +{ + /* Current heading in degrees, in compass units (0..360, 0=north) */ + int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi); + /* Current throttle setting in integer percent, 0 to 100 */ + // is a 16bit unsigned int but supposed to be from 0 to 100?? + uint16_t throttle; +#ifdef COMMAND_THRUST + throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100); +#elif defined COMMAND_THROTTLE + throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100); +#endif + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, + stateGetAirspeed_f(), + stateGetHorizontalSpeedNorm_f(), // groundspeed + heading, + throttle, + stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL + stateGetSpeedNed_f()->z); // climb rate + MAVLinkSendMessage(); +} From 91bb2cd1f2ca68ca28fb16a2f24bfc06588e95a1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 14 Nov 2015 23:53:47 +0100 Subject: [PATCH 03/12] [modules] mavlink: parse MISSION_ITEM message makes it possible to change waypoints when using the rotorcraft firmware --- sw/airborne/modules/datalink/mavlink.c | 51 ++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index a71980a5d2..cdff19cf7e 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -65,6 +65,7 @@ static uint8_t mavlink_params_idx = NB_SETTING; /**< Transmitting parameters ind static char mavlink_param_names[NB_SETTING][16+1] = SETTINGS_NAMES_SHORT; static uint8_t custom_version[8]; /**< first 8 bytes (16 chars) of GIT SHA1 */ +static inline void mavlink_send_mission_ack(void); static inline void mavlink_send_heartbeat(void); static inline void mavlink_send_sys_status(void); static inline void mavlink_send_attitude(void); @@ -283,6 +284,47 @@ void mavlink_event(void) } break; +#ifndef AP + case MAVLINK_MSG_ID_MISSION_ITEM: { + /* change waypoint: only available when using waypoint API (rotorcraft firmware) + * This uses the waypoint_set_x functions (opposed to waypoint_move_x), + * meaning it doesn't send WP_MOVED Paparazzi messages. + */ + mavlink_mission_item_t mission_item; + mavlink_msg_mission_item_decode(&msg, &mission_item); + + if (mission_item.target_system == mavlink_system.sysid) { + if (mission_item.seq < NB_WAYPOINT) { + if (mission_item.frame == MAV_FRAME_GLOBAL_INT) { + struct LlaCoor_i lla; + lla.lat = mission_item.x; // lattitude in degrees*1e7 + lla.lat = mission_item.y; // longitude in degrees*1e7 + lla.alt = mission_item.z * 1e3; // altitude in millimeters + waypoint_set_lla(mission_item.seq, &lla); + mavlink_send_mission_ack(); + } + else if (mission_item.frame == MAV_FRAME_GLOBAL) { + struct LlaCoor_i lla; + lla.lat = mission_item.x * 1e7; // lattitude in degrees*1e7 + lla.lat = mission_item.y * 1e7; // longitude in degrees*1e7 + lla.alt = mission_item.z * 1e3; // altitude in millimeters + waypoint_set_lla(mission_item.seq, &lla); + mavlink_send_mission_ack(); + } + else if (mission_item.frame == MAV_FRAME_LOCAL_ENU) { + struct EnuCoor_f enu; + enu.x = mission_item.x; + enu.y = mission_item.y; + enu.z = mission_item.z; + waypoint_set_enu(mission_item.seq, &enu); + mavlink_send_mission_ack(); + } + } + } + } + break; +#endif + default: //Do nothing //printf("Received message with id: %d\r\n", msg.msgid); @@ -293,6 +335,15 @@ void mavlink_event(void) } } +static inline void mavlink_send_mission_ack(void) +{ + mavlink_msg_mission_ack_send(MAVLINK_COMM_0, + mavlink_system.sysid, + mavlink_system.compid, + MAV_MISSION_ACCEPTED); + MAVLinkSendMessage(); +} + /** * Send a heartbeat */ From 1791493e8081c6016084e9a0f1975399e63f49ee Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Nov 2015 00:39:46 +0100 Subject: [PATCH 04/12] [mavlink] add paparazzi specific SCRIPT_ITEM messages - makes it possible to get and change the flight plan blocks alternative implementation to #1407 --- sw/airborne/modules/datalink/mavlink.c | 38 ++++++++++++++++++++++++++ sw/tools/generators/gen_flight_plan.ml | 4 +-- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index cdff19cf7e..7f9ab2f7fa 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -325,6 +325,44 @@ void mavlink_event(void) break; #endif + /* Paparazzi dialect specific SCRIPT message */ + /* request for script/block list, answer with number of blocks */ + case MAVLINK_MSG_ID_SCRIPT_REQUEST_LIST: { + mavlink_script_request_list_t req; + mavlink_msg_script_request_list_decode(&msg, &req); + if (req.target_system == mavlink_system.sysid) { + mavlink_msg_script_count_send(MAVLINK_COMM_0, msg.sysid, msg.compid, NB_BLOCK); + MAVLinkSendMessage(); + } + } + break; + + /* request script/block, answer with SCRIPT_ITEM (block) */ + case MAVLINK_MSG_ID_SCRIPT_REQUEST: { + mavlink_script_request_t req; + mavlink_msg_script_request_decode(&msg, &req); + if (req.target_system == mavlink_system.sysid && req.seq < NB_BLOCK) { + static const char *blocks[] = FP_BLOCKS; + char block_name[50]; + strncpy(block_name, blocks[req.seq], 49); // String containing the name of the block + uint8_t len = strlen(blocks[req.seq]); // Length of the block name + mavlink_msg_script_item_send(MAVLINK_COMM_0, msg.sysid, msg.compid, + req.seq, len, block_name); + MAVLinkSendMessage(); + } + } + break; + + /* got script item, change block */ + case MAVLINK_MSG_ID_SCRIPT_ITEM: { + mavlink_script_item_t block; + mavlink_msg_script_item_decode(&msg, &block); + if (block.target_system == mavlink_system.sysid && block.seq < NB_BLOCK) { + nav_goto_block(block.seq); + } + } + break; + default: //Do nothing //printf("Received message with id: %d\r\n", msg.msgid); diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index 766eabcac0..aaa71b2cd2 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -908,8 +908,8 @@ let () = Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); Xml2h.define "FP_BLOCKS" "{ \\"; - List.iter (fun b -> printf " { \"%s\" }, \\\n" (ExtXml.attrib b "name")) blocks; - lprintf "};\n"; + List.iter (fun b -> printf " \"%s\" , \\\n" (ExtXml.attrib b "name")) blocks; + lprintf "} \n"; Xml2h.define "NB_BLOCK" (string_of_int (List.length blocks)); Xml2h.define "GROUND_ALT" (sof !ground_alt); From eb5e8d144d26437430918a750b8b9bed2258df42 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Nov 2015 13:00:39 +0100 Subject: [PATCH 05/12] [mavlink] fix typos --- sw/airborne/modules/datalink/mavlink.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 7f9ab2f7fa..f38101584a 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -298,7 +298,7 @@ void mavlink_event(void) if (mission_item.frame == MAV_FRAME_GLOBAL_INT) { struct LlaCoor_i lla; lla.lat = mission_item.x; // lattitude in degrees*1e7 - lla.lat = mission_item.y; // longitude in degrees*1e7 + lla.lon = mission_item.y; // longitude in degrees*1e7 lla.alt = mission_item.z * 1e3; // altitude in millimeters waypoint_set_lla(mission_item.seq, &lla); mavlink_send_mission_ack(); @@ -306,7 +306,7 @@ void mavlink_event(void) else if (mission_item.frame == MAV_FRAME_GLOBAL) { struct LlaCoor_i lla; lla.lat = mission_item.x * 1e7; // lattitude in degrees*1e7 - lla.lat = mission_item.y * 1e7; // longitude in degrees*1e7 + lla.lon = mission_item.y * 1e7; // longitude in degrees*1e7 lla.alt = mission_item.z * 1e3; // altitude in millimeters waypoint_set_lla(mission_item.seq, &lla); mavlink_send_mission_ack(); From 6d71b9596a7cb82a63cd9150cb1a9a18600fe1d9 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Nov 2015 14:48:50 +0100 Subject: [PATCH 06/12] [mavlink] send waypoints as lla for rotorcraft sending lat/lon as float is actually a bad idea, but it seems that most GCSs don't understand the MISSION_ITEM_INT or LOCAL_FRAME_ENU --- sw/airborne/modules/datalink/mavlink.c | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index f38101584a..76070e0bdc 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -266,6 +266,8 @@ void mavlink_event(void) if (req.target_system == mavlink_system.sysid) { if (req.seq < NB_WAYPOINT) { +#ifdef AP + /* for fixedwing firmware send as LOCAL_ENU for now */ mavlink_msg_mission_item_send(MAVLINK_COMM_0, msg.sysid, msg.compid, @@ -278,6 +280,25 @@ void mavlink_event(void) WaypointX(req.seq), WaypointY(req.seq), WaypointAlt(req.seq)); +#else + /* for rotorcraft firmware use waypoint API and send as lat/lon */ + /* sending lat/lon as float is actually a bad idea, + * but it seems that most GCSs don't understand the MISSION_ITEM_INT + */ + struct LlaCoor_i *lla = waypoint_get_lla(req.seq); + mavlink_msg_mission_item_send(MAVLINK_COMM_0, + msg.sysid, + msg.compid, + req.seq, + MAV_FRAME_GLOBAL, + MAV_CMD_NAV_WAYPOINT, + 0, // current + 0, // autocontinue + 0, 0, 0, 0, // params + (float)lla->lat / 1e7, + (float)lla->lon / 1e7, + (float)lla->alt / 1e3); +#endif MAVLinkSendMessage(); } } From 6fb9777c7f328920afe596cb6db69f93e6b9ded7 Mon Sep 17 00:00:00 2001 From: LodewijkSikkel Date: Sun, 15 Nov 2015 16:22:25 +0100 Subject: [PATCH 07/12] [mavlink] add missionlib files from Lodewijk --- .../modules/datalink/missionlib/blocks.c | 155 +++++++++++++ .../modules/datalink/missionlib/blocks.h | 42 ++++ .../datalink/missionlib/mission_manager.c | 55 +++++ .../datalink/missionlib/mission_manager.h | 91 ++++++++ .../modules/datalink/missionlib/waypoints.c | 215 ++++++++++++++++++ .../modules/datalink/missionlib/waypoints.h | 42 ++++ 6 files changed, 600 insertions(+) create mode 100644 sw/airborne/modules/datalink/missionlib/blocks.c create mode 100644 sw/airborne/modules/datalink/missionlib/blocks.h create mode 100644 sw/airborne/modules/datalink/missionlib/mission_manager.c create mode 100644 sw/airborne/modules/datalink/missionlib/mission_manager.h create mode 100644 sw/airborne/modules/datalink/missionlib/waypoints.c create mode 100644 sw/airborne/modules/datalink/missionlib/waypoints.h diff --git a/sw/airborne/modules/datalink/missionlib/blocks.c b/sw/airborne/modules/datalink/missionlib/blocks.c new file mode 100644 index 0000000000..541024f3d5 --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/blocks.c @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2015 Lodewijk Sikkel + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/datalink/missionlib/blocks.c + * @brief PPRZ specific mission block implementation + */ + +// Include own header +#include "modules/datalink/missionlib/blocks.h" + +#include +#include + +#include "modules/datalink/mavlink.h" +#include "modules/datalink/missionlib/mission_manager.h" + +#include "subsystems/navigation/common_flight_plan.h" + +static void mavlink_send_block_count(void) +{ + mavlink_message_t msg; + mavlink_script_count_t block_count; + block_count.target_system = mission_mgr.rem_sysid; + block_count.target_component = mission_mgr.rem_compid; + block_count.count = NB_BLOCK; // From the generated flight plan + + mavlink_msg_script_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, + &block_count); // encode the block count message + + MAVLINK_DEBUG("Sent BLOCK_COUNT message\n"); + mavlink_send_message(&msg); +} + +static void mavlink_send_block(uint16_t seq) +{ + if (seq < NB_BLOCK) { // Due to indexing + mavlink_message_t msg; + mavlink_script_item_t block_item; + block_item.seq = seq; + char *blocks[] = FP_BLOCKS; + block_item.len = (uint8_t)strlen(blocks[seq]); // Length of the block name + strcpy(block_item.name, blocks[seq]); // String containing the name of the block + block_item.target_system = mission_mgr.rem_sysid; + block_item.target_component = mission_mgr.rem_compid; + + mavlink_msg_script_item_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &block_item); + + MAVLINK_DEBUG("Sent BLOCK_ITEM message\n"); + mavlink_send_message(&msg); + } else { + MAVLINK_DEBUG("ERROR: Block index out of bounds\n"); + } +} + +void mavlink_block_init(void) +{ +} + +void mavlink_block_cb(uint16_t current_block) +{ + mavlink_send_block(current_block); // send the current block seq + + mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, &timer_cb); // wait for ack +} + +void mavlink_block_message_handler(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_SCRIPT_REQUEST_LIST: { + MAVLINK_DEBUG("Received BLOCK_REQUEST_LIST message\n"); + mavlink_script_request_list_t block_request_list_msg; + mavlink_msg_script_request_list_decode(msg, + &block_request_list_msg); // Cast the incoming message to a block_request_list_msg + if (block_request_list_msg.target_system == mavlink_system.sysid) { + if (mission_mgr.state == STATE_IDLE) { + if (NB_BLOCK > 0) { + mission_mgr.state = STATE_SEND_LIST; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); + mission_mgr.seq = 0; + mission_mgr.rem_sysid = msg->sysid; + mission_mgr.rem_compid = msg->compid; + } + mavlink_send_block_count(); + + mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, + &timer_cb); // Register the timeout timer (it is continuous so it needs to be cancelled after triggering) + } else { + // TODO: Handle case when the state is not IDLE + } + } else { + // TODO: Handle remote system id mismatch + } + + break; + } + + case MAVLINK_MSG_ID_SCRIPT_REQUEST: { + MAVLINK_DEBUG("Received BLOCK_REQUEST message\n"); + mavlink_script_request_t block_request_msg; + mavlink_msg_script_request_decode(msg, &block_request_msg); // Cast the incoming message to a block_request_msg + if (block_request_msg.target_system == mavlink_system.sysid) { + // Handle only cases in which the entire list is request, the block is sent again, or the next block was sent + if ((mission_mgr.state == STATE_SEND_LIST && block_request_msg.seq == 0) || + (mission_mgr.state == STATE_SEND_ITEM && (block_request_msg.seq == mission_mgr.seq + || block_request_msg.seq == mission_mgr.seq + 1))) { + sys_time_cancel_timer(mission_mgr.timer_id); // Cancel the timeout timer + + mission_mgr.state = STATE_SEND_ITEM; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); + mission_mgr.seq = block_request_msg.seq; + + mavlink_send_block(mission_mgr.seq); + + mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, &timer_cb); // Register the timeout timer + } else { + // TODO: Handle cases for which the above condition does not hold + } + } else { + // TODO: Handle remote system id mismatch + } + + break; + } + + case MAVLINK_MSG_ID_SCRIPT_ITEM: { + MAVLINK_DEBUG("Received BLOCK_ITEM message\n"); + mavlink_script_item_t block_item_msg; + mavlink_msg_script_item_decode(msg, &block_item_msg); // Cast the incoming message to a block_item_msg + if (block_item_msg.target_system == mavlink_system.sysid) { + if (mission_mgr.state == STATE_IDLE) { // Only handle incoming block item messages if there a not currently being sent + nav_goto_block((uint8_t)block_item_msg.seq); // Set the current block + } + } + } + } +} diff --git a/sw/airborne/modules/datalink/missionlib/blocks.h b/sw/airborne/modules/datalink/missionlib/blocks.h new file mode 100644 index 0000000000..a23b89915a --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/blocks.h @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2015 Lodewijk Sikkel + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/datalink/missionlib/blocks.h + * @brief PPRZ specific mission block implementation + */ + +#ifndef MISSIONLIB_BLOCKS_H +#define MISSIONLIB_BLOCKS_H + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "generated/flight_plan.h" +#include "mavlink/paparazzi/mavlink.h" + +extern void mavlink_block_init(void); +extern void mavlink_block_cb(uint16_t current_block); +extern void mavlink_block_message_handler(const mavlink_message_t *msg); + +#endif // MISSIONLIB_BLOCKS_H diff --git a/sw/airborne/modules/datalink/missionlib/mission_manager.c b/sw/airborne/modules/datalink/missionlib/mission_manager.c new file mode 100644 index 0000000000..403f086372 --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.c @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015 Lodewijk Sikkel + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/datalink/missionlib/mission_manager.c +* @brief Common functions used within the mission library +*/ + +// Include own header +#include "modules/datalink/missionlib/mission_manager.h" + +#include "modules/datalink/mavlink.h" +#include "modules/datalink/missionlib/blocks.h" +#include "modules/datalink/missionlib/waypoints.h" + +void mavlink_mission_init(mavlink_mission_mgr *mission_mgr) +{ + mavlink_wp_init(); + + mission_mgr->seq = 0; +} + +void mavlink_mission_message_handler(const mavlink_message_t *msg) +{ + mavlink_block_message_handler(msg); + + mavlink_wp_message_handler(msg); + + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: { + MAVLINK_DEBUG("Received MISSION_ACK message\n"); + sys_time_cancel_timer(mission_mgr.timer_id); // Cancel the timeout timer + mission_mgr.state = STATE_IDLE; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); + } + } +} diff --git a/sw/airborne/modules/datalink/missionlib/mission_manager.h b/sw/airborne/modules/datalink/missionlib/mission_manager.h new file mode 100644 index 0000000000..b19b221447 --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.h @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2015 Lodewijk Sikkel + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/datalink/missionlib/mission_manager.h +* @brief Common functions used within the mission library, blocks and +* waypoints cannot be send simultaneously (which should not +* matter) +*/ + +#ifndef MISSIONLIB_COMMON_H +#define MISSIONLIB_COMMON_H + +#include + +// #include "firmwares/rotorcraft/navigation.h" +#include "mcu_periph/sys_time.h" + +#include "generated/flight_plan.h" +#include "mavlink/paparazzi/mavlink.h" + +#ifndef MAVLINK_TIMEOUT +#define MAVLINK_TIMEOUT 15 // as in MAVLink waypoint convention +#endif + +// State machine +enum MAVLINK_MISSION_MGR_STATES { + STATE_IDLE = 0, + STATE_SEND_LIST, + STATE_SEND_ITEM, + STATE_WRITE_ITEM, // only for updating waypoints +}; + +struct mavlink_mission_mgr { + mavlink_mission_item_t waypoints[NB_WAYPOINT]; // Array containing the waypoints in global coordinates + uint8_t current_block; // Counter that holds the index of the current block + enum MAVLINK_MISSION_MGR_STATES state; // The current state of the mission handler + uint16_t seq; // Sequence id (position of the current item on the list) + uint8_t rem_sysid; // Remote system id + uint8_t rem_compid; // Remote component id + int timer_id; // Timer id +}; + +typedef struct mavlink_mission_mgr mavlink_mission_mgr; + +extern mavlink_mission_mgr mission_mgr; + +// Timer callback function +static inline void timer_cb(uint8_t id) +{ + sys_time_cancel_timer(id); // Cancel the timer that triggered the timeout event + mission_mgr.state = STATE_IDLE; + MAVLINK_DEBUG("ERROR: Request timed out!\n"); + // TODO: Handle timeout retries, for now just assume no retries +} + +static inline void sendMissionAck() +{ + mavlink_message_t msg; + mavlink_mission_ack_t mission_ack; + mission_ack.target_system = mission_mgr.rem_sysid; + mission_ack.target_component = mission_mgr.rem_compid; + mission_ack.type = MAV_MISSION_ACCEPTED; + mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, + &mission_ack); // encode the ack message + MAVLINK_DEBUG("Sent MISSION_ACK message\n"); + mavlink_send_message(&msg); +} + +extern void mavlink_mission_init(mavlink_mission_mgr *mission_mgr); +extern void mavlink_mission_message_handler(const mavlink_message_t *msg); + +#endif diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.c b/sw/airborne/modules/datalink/missionlib/waypoints.c new file mode 100644 index 0000000000..9acf40eb81 --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/waypoints.c @@ -0,0 +1,215 @@ +/* + * Copyright (C) 2015 Lodewijk Sikkel + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/datalink/missionlib/waypoints.c + * @brief Improvement of the missionlib implementation of the waypoint protocol, + * truly global waypoints are used such that they will not be relocated after you + * run GeoInit. + */ + +// Include own header +#include "modules/datalink/missionlib/waypoints.h" + +#include +#include + +#include "subsystems/navigation/waypoints.h" +//#include "subsystems/navigation/common_nav.h" // for fixed-wing aircraft + +#include "modules/datalink/mavlink.h" +#include "modules/datalink/missionlib/mission_manager.h" + +static void mavlink_update_wp_list(void) +{ + // Store the waypoints in a mission item + if (NB_WAYPOINT > 0) { +// for (uint8_t i = 0; i < NB_WAYPOINT; i++) { +// mavlink_mission_item_t mission_item; +// +// /* +// * Convert ENU waypoint to UTM +// * May be removed, but nav_move_waypoint() uses UTM coordinates in case of fixed-wing aircraft +// */ +// struct UtmCoor_f utm; +// utm.east = waypoints[i].x + nav_utm_east0; +// utm.north = waypoints[i].y + nav_utm_north0; +// utm.alt = waypoints[i].a; +// utm.zone = nav_utm_zone0; +// +// // Convert UTM waypoint to LLA +// struct LlaCoor_f lla; +// lla_of_utm_f(&lla, &utm); +// mission_item.x = lla.lat; // lattitude +// mission_item.y = lla.lon; // longtitude +// mission_item.z = lla.alt; // altitude +// mission_item.seq = i; +// +// mission_mgr.waypoints[i] = mission_item; +// } + for (uint8_t i = 0; i < NB_WAYPOINT; i++) { + mavlink_mission_item_t mission_item; + // waypoint_set_global_flag(i); + if (waypoint_is_global(i)) { + MAVLINK_DEBUG("Waypoint(%d): is global\n", i); + } else { + MAVLINK_DEBUG("Waypoint(%d): is NOT global\n", i); + } + waypoint_globalize(i); + mission_item.x = (float)waypoint_get_lla(i)->lat * 1e-7; // lattitude + mission_item.y = (float)waypoint_get_lla(i)->lon * 1e-7; // longtitude + mission_item.z = (float)waypoint_get_lla(i)->alt * 1e-3; // altitude + + MAVLINK_DEBUG("WP: %f, %f, %f\n", mission_item.x, mission_item.y, mission_item.z); + + mission_item.seq = i; + mission_mgr.waypoints[i] = mission_item; + } + } else { + MAVLINK_DEBUG("ERROR: The waypoint array is empty\n"); + } +} + +static void mavlink_send_wp_count(void) +{ + mavlink_message_t msg; + mavlink_mission_count_t wp_count; + wp_count.target_system = mission_mgr.rem_sysid; + wp_count.target_component = mission_mgr.rem_compid; + wp_count.count = NB_WAYPOINT; // From the generated flight plan + + mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, + &wp_count); // encode the block count message + + MAVLINK_DEBUG("Sent WP_COUNT message\n"); + mavlink_send_message(&msg); +} + +static void mavlink_send_wp(uint16_t seq) +{ + if (seq < NB_WAYPOINT) { // Due to indexing + mavlink_message_t msg; + mavlink_mission_item_t *mission_item = &(mission_mgr.waypoints[seq]); // Copy the reference to the mission item + + mission_item->target_system = mission_mgr.rem_sysid; + mission_item->target_component = mission_mgr.rem_compid; + + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_system.compid, &msg, mission_item); + + MAVLINK_DEBUG("Sent MISSION_ITEM message\n"); + mavlink_send_message(&msg); + } else { + MAVLINK_DEBUG("ERROR: Wp index out of bounds\n"); + } +} + +void mavlink_wp_init(void) +{ + mavlink_update_wp_list(); +} + +void mavlink_wp_message_handler(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + MAVLINK_DEBUG("Received MISSION_REQUEST_LIST message\n"); + mavlink_mission_request_list_t mission_request_list_msg; + mavlink_msg_mission_request_list_decode(msg, + &mission_request_list_msg); // Cast the incoming message to a mission_request_list_msg + if (mission_request_list_msg.target_system == mavlink_system.sysid) { + if (mission_mgr.state == STATE_IDLE) { + if (NB_WAYPOINT > 0) { + mission_mgr.state = STATE_SEND_LIST; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); + mission_mgr.seq = 0; + mission_mgr.rem_sysid = msg->sysid; + mission_mgr.rem_compid = msg->compid; + } + mavlink_send_wp_count(); + + mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, + &timer_cb); // Register the timeout timer (it is continuous so it needs to be cancelled after triggering) + } else { + // TODO: Handle case when the state is not IDLE + } + } else { + // TODO: Handle remote system id mismatch + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST: { + MAVLINK_DEBUG("Received MISSION_REQUEST message\n"); + mavlink_mission_request_t mission_request_msg; + mavlink_msg_mission_request_decode(msg, &mission_request_msg); // Cast the incoming message to a mission_request_msg + if (mission_request_msg.target_system == mavlink_system.sysid) { + if ((mission_mgr.state == STATE_SEND_LIST && mission_request_msg.seq == 0) || // Send the first waypoint + (mission_mgr.state == STATE_SEND_ITEM && (mission_request_msg.seq == mission_mgr.seq + || // Send the current waypoint again + mission_request_msg.seq == mission_mgr.seq + 1))) { // Send the next waypoint + sys_time_cancel_timer(mission_mgr.timer_id); // Cancel the timeout timer + + mission_mgr.state = STATE_SEND_ITEM; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); + mission_mgr.seq = mission_request_msg.seq; + + mavlink_update_wp_list(); // Update the waypoint list + + mavlink_send_wp(mission_mgr.seq); + + mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, &timer_cb); // Register the timeout timer + } else { + // TODO: Handle cases for which the above condition does not hold + } + } else { + // TODO: Handle remote system id mismatch + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_ITEM: { + MAVLINK_DEBUG("Received MISSION_ITEM message\n"); + mavlink_mission_item_t mission_item_msg; + mavlink_msg_mission_item_decode(msg, &mission_item_msg); // Cast the incoming message to a mission_item_msg + + if (mission_item_msg.target_system == mavlink_system.sysid) { + if (mission_mgr.state == STATE_IDLE) { // Only handle incoming mission item messages if there a not currently being sent + struct LlaCoor_i lla; + lla.lat = (int32_t)(mission_item_msg.x * 1e7); // lattitude in degrees*1e7 + lla.lon = (int32_t)(mission_item_msg.y * 1e7); // longitude in degrees*1e7 + lla.alt = (int32_t)(mission_item_msg.z * 1e3); // altitude in millimeters + +// struct UtmCoor_f utm; +// utm.zone = nav_utm_zone0; +// utm_of_lla_f(&utm, &lla); + MAVLINK_DEBUG("Received WP(%d): %d, %d, %d\n", mission_item_msg.seq, lla.lat, lla.lon, lla.alt); + // Set the new waypoint + waypoint_move_lla(mission_item_msg.seq, &lla); // move the waypoint with the id equal to seq, only for rotorcraft +// nav_move_waypoint(mission_item_msg.seq, utm.east, utm.north, utm.alt); + + sendMissionAck(); + } + } + } + } +} diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.h b/sw/airborne/modules/datalink/missionlib/waypoints.h new file mode 100644 index 0000000000..84c2ca2125 --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/waypoints.h @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2015 Lodewijk Sikkel + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#ifndef MISSIONLIB_WAYPOINTS_H +#define MISSIONLIB_WAYPOINTS_H + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "generated/flight_plan.h" +#include "mavlink/paparazzi/mavlink.h" + +// Block storage struct +#ifndef MAVLINK_MAX_WP_COUNT +#define MAVLINK_MAX_WP_COUNT NB_WAYPOINT +#endif + +extern void mavlink_wp_init(void); +extern void mavlink_wp_message_handler(const mavlink_message_t *msg); + +#endif // MISSIONLIB_WAYPOINTS_H \ No newline at end of file From b5ac3643657eac0c1cb43f07a42ffeb7b4603fb5 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Nov 2015 19:12:22 +0100 Subject: [PATCH 08/12] [mavlink] integrate and improve missionlib - don't use timer callbacks as they are called from system context - remove duplicate waypoints from mission manager - waypoint protocol: - start waypoint write/update transaction upon receiving MISSION_COUNT message - request waypoints Seems to work fine with e.g. QGroundControl now --- conf/modules/mavlink.xml | 3 + sw/airborne/modules/datalink/mavlink.c | 349 ++++++------------ sw/airborne/modules/datalink/mavlink.h | 8 + .../modules/datalink/missionlib/blocks.c | 93 ++--- .../modules/datalink/missionlib/blocks.h | 11 +- .../datalink/missionlib/mission_manager.c | 81 +++- .../datalink/missionlib/mission_manager.h | 42 +-- .../modules/datalink/missionlib/waypoints.c | 327 +++++++++------- .../modules/datalink/missionlib/waypoints.h | 20 +- 9 files changed, 432 insertions(+), 502 deletions(-) diff --git a/conf/modules/mavlink.xml b/conf/modules/mavlink.xml index 7a914eec1b..450333f035 100644 --- a/conf/modules/mavlink.xml +++ b/conf/modules/mavlink.xml @@ -16,6 +16,9 @@ + + + MAVLINK_PORT ?= UART1 MAVLINK_PORT_UPPER=$(shell echo $(MAVLINK_PORT) | tr a-z A-Z) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 76070e0bdc..0c9ac233b8 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -45,13 +45,7 @@ #include "subsystems/radio_control.h" #endif -// for waypoints, include correct header until we have unified API -#ifdef AP -#include "subsystems/navigation/common_nav.h" -#else -#include "firmwares/rotorcraft/navigation.h" -#endif -#include "generated/flight_plan.h" +#include "modules/datalink/missionlib/mission_manager.h" // for UINT16_MAX #include @@ -65,7 +59,10 @@ static uint8_t mavlink_params_idx = NB_SETTING; /**< Transmitting parameters ind static char mavlink_param_names[NB_SETTING][16+1] = SETTINGS_NAMES_SHORT; static uint8_t custom_version[8]; /**< first 8 bytes (16 chars) of GIT SHA1 */ -static inline void mavlink_send_mission_ack(void); +mavlink_mission_mgr mission_mgr; + +void mavlink_common_message_handler(const mavlink_message_t *msg); + static inline void mavlink_send_heartbeat(void); static inline void mavlink_send_sys_status(void); static inline void mavlink_send_attitude(void); @@ -94,6 +91,8 @@ void mavlink_init(void) mavlink_system.compid = MAV_COMP_ID_MISSIONPLANNER; // Component/Subsystem ID, 1-255 get_pprz_git_version(custom_version); + + mavlink_mission_init(&mission_mgr); } /** @@ -117,6 +116,8 @@ void mavlink_periodic(void) RunOnceEvery(33, mavlink_send_gps_global_origin()); RunOnceEvery(10, mavlink_send_gps_status()); RunOnceEvery(10, mavlink_send_vfr_hud()); + + mavlink_mission_periodic(); } static int16_t settings_idx_from_param_id(char *param_id) @@ -158,249 +159,105 @@ void mavlink_event(void) // When we receive a message if (mavlink_parse_char(MAVLINK_COMM_0, test, &msg, &status)) { - 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; - - /* request for mission list, answer with number of waypoints */ - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t req; - mavlink_msg_mission_request_list_decode(&msg, &req); - - if (req.target_system == mavlink_system.sysid) { - mavlink_msg_mission_count_send(MAVLINK_COMM_0, - msg.sysid, - msg.compid, - NB_WAYPOINT); - MAVLinkSendMessage(); - } - } - break; - - /* request for mission item, answer with waypoint */ - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t req; - mavlink_msg_mission_request_decode(&msg, &req); - - if (req.target_system == mavlink_system.sysid) { - if (req.seq < NB_WAYPOINT) { -#ifdef AP - /* for fixedwing firmware send as LOCAL_ENU for now */ - mavlink_msg_mission_item_send(MAVLINK_COMM_0, - msg.sysid, - msg.compid, - req.seq, - MAV_FRAME_LOCAL_ENU, - MAV_CMD_NAV_WAYPOINT, - 0, // current - 0, // autocontinue - 0, 0, 0, 0, // params - WaypointX(req.seq), - WaypointY(req.seq), - WaypointAlt(req.seq)); -#else - /* for rotorcraft firmware use waypoint API and send as lat/lon */ - /* sending lat/lon as float is actually a bad idea, - * but it seems that most GCSs don't understand the MISSION_ITEM_INT - */ - struct LlaCoor_i *lla = waypoint_get_lla(req.seq); - mavlink_msg_mission_item_send(MAVLINK_COMM_0, - msg.sysid, - msg.compid, - req.seq, - MAV_FRAME_GLOBAL, - MAV_CMD_NAV_WAYPOINT, - 0, // current - 0, // autocontinue - 0, 0, 0, 0, // params - (float)lla->lat / 1e7, - (float)lla->lon / 1e7, - (float)lla->alt / 1e3); -#endif - MAVLinkSendMessage(); - } - } - } - break; - -#ifndef AP - case MAVLINK_MSG_ID_MISSION_ITEM: { - /* change waypoint: only available when using waypoint API (rotorcraft firmware) - * This uses the waypoint_set_x functions (opposed to waypoint_move_x), - * meaning it doesn't send WP_MOVED Paparazzi messages. - */ - mavlink_mission_item_t mission_item; - mavlink_msg_mission_item_decode(&msg, &mission_item); - - if (mission_item.target_system == mavlink_system.sysid) { - if (mission_item.seq < NB_WAYPOINT) { - if (mission_item.frame == MAV_FRAME_GLOBAL_INT) { - struct LlaCoor_i lla; - lla.lat = mission_item.x; // lattitude in degrees*1e7 - lla.lon = mission_item.y; // longitude in degrees*1e7 - lla.alt = mission_item.z * 1e3; // altitude in millimeters - waypoint_set_lla(mission_item.seq, &lla); - mavlink_send_mission_ack(); - } - else if (mission_item.frame == MAV_FRAME_GLOBAL) { - struct LlaCoor_i lla; - lla.lat = mission_item.x * 1e7; // lattitude in degrees*1e7 - lla.lon = mission_item.y * 1e7; // longitude in degrees*1e7 - lla.alt = mission_item.z * 1e3; // altitude in millimeters - waypoint_set_lla(mission_item.seq, &lla); - mavlink_send_mission_ack(); - } - else if (mission_item.frame == MAV_FRAME_LOCAL_ENU) { - struct EnuCoor_f enu; - enu.x = mission_item.x; - enu.y = mission_item.y; - enu.z = mission_item.z; - waypoint_set_enu(mission_item.seq, &enu); - mavlink_send_mission_ack(); - } - } - } - } - break; -#endif - - /* Paparazzi dialect specific SCRIPT message */ - /* request for script/block list, answer with number of blocks */ - case MAVLINK_MSG_ID_SCRIPT_REQUEST_LIST: { - mavlink_script_request_list_t req; - mavlink_msg_script_request_list_decode(&msg, &req); - if (req.target_system == mavlink_system.sysid) { - mavlink_msg_script_count_send(MAVLINK_COMM_0, msg.sysid, msg.compid, NB_BLOCK); - MAVLinkSendMessage(); - } - } - break; - - /* request script/block, answer with SCRIPT_ITEM (block) */ - case MAVLINK_MSG_ID_SCRIPT_REQUEST: { - mavlink_script_request_t req; - mavlink_msg_script_request_decode(&msg, &req); - if (req.target_system == mavlink_system.sysid && req.seq < NB_BLOCK) { - static const char *blocks[] = FP_BLOCKS; - char block_name[50]; - strncpy(block_name, blocks[req.seq], 49); // String containing the name of the block - uint8_t len = strlen(blocks[req.seq]); // Length of the block name - mavlink_msg_script_item_send(MAVLINK_COMM_0, msg.sysid, msg.compid, - req.seq, len, block_name); - MAVLinkSendMessage(); - } - } - break; - - /* got script item, change block */ - case MAVLINK_MSG_ID_SCRIPT_ITEM: { - mavlink_script_item_t block; - mavlink_msg_script_item_decode(&msg, &block); - if (block.target_system == mavlink_system.sysid && block.seq < NB_BLOCK) { - nav_goto_block(block.seq); - } - } - break; - - default: - //Do nothing - //printf("Received message with id: %d\r\n", msg.msgid); - break; - } + mavlink_common_message_handler(&msg); + mavlink_mission_message_handler(&msg); } - } } -static inline void mavlink_send_mission_ack(void) +void mavlink_common_message_handler(const mavlink_message_t *msg) { - mavlink_msg_mission_ack_send(MAVLINK_COMM_0, - mavlink_system.sysid, - mavlink_system.compid, - MAV_MISSION_ACCEPTED); - MAVLinkSendMessage(); + 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; + } } /** diff --git a/sw/airborne/modules/datalink/mavlink.h b/sw/airborne/modules/datalink/mavlink.h index ac4c0145f3..85a8272a93 100644 --- a/sw/airborne/modules/datalink/mavlink.h +++ b/sw/airborne/modules/datalink/mavlink.h @@ -40,6 +40,14 @@ #endif #include "mcu_periph/uart.h" +#ifndef MAVLINK_DEBUG +#define MAVLINK_DEBUG(...) {} +#endif + +#if MAVLINK_DEBUG == printf +#include +#endif + /* * MAVLink description before main MAVLink include */ diff --git a/sw/airborne/modules/datalink/missionlib/blocks.c b/sw/airborne/modules/datalink/missionlib/blocks.c index 541024f3d5..1f264cb18f 100644 --- a/sw/airborne/modules/datalink/missionlib/blocks.c +++ b/sw/airborne/modules/datalink/missionlib/blocks.c @@ -24,72 +24,50 @@ * @brief PPRZ specific mission block implementation */ -// Include own header #include "modules/datalink/missionlib/blocks.h" - -#include -#include - -#include "modules/datalink/mavlink.h" #include "modules/datalink/missionlib/mission_manager.h" +#include "modules/datalink/mavlink.h" + +// include mavlink headers, but ignore some warnings +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wswitch-default" +#include "mavlink/paparazzi/mavlink.h" +#pragma GCC diagnostic pop #include "subsystems/navigation/common_flight_plan.h" +#include "generated/flight_plan.h" static void mavlink_send_block_count(void) { - mavlink_message_t msg; - mavlink_script_count_t block_count; - block_count.target_system = mission_mgr.rem_sysid; - block_count.target_component = mission_mgr.rem_compid; - block_count.count = NB_BLOCK; // From the generated flight plan - - mavlink_msg_script_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, - &block_count); // encode the block count message - - MAVLINK_DEBUG("Sent BLOCK_COUNT message\n"); - mavlink_send_message(&msg); + mavlink_msg_script_count_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid, NB_BLOCK); + MAVLinkSendMessage(); + MAVLINK_DEBUG("Sent BLOCK_COUNT message: count %i\n", NB_BLOCK); } -static void mavlink_send_block(uint16_t seq) +void mavlink_send_block(uint16_t seq) { if (seq < NB_BLOCK) { // Due to indexing - mavlink_message_t msg; - mavlink_script_item_t block_item; - block_item.seq = seq; - char *blocks[] = FP_BLOCKS; - block_item.len = (uint8_t)strlen(blocks[seq]); // Length of the block name - strcpy(block_item.name, blocks[seq]); // String containing the name of the block - block_item.target_system = mission_mgr.rem_sysid; - block_item.target_component = mission_mgr.rem_compid; - - mavlink_msg_script_item_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &block_item); - - MAVLINK_DEBUG("Sent BLOCK_ITEM message\n"); - mavlink_send_message(&msg); + static const char *blocks[] = FP_BLOCKS; + char block_name[50]; + strncpy(block_name, blocks[seq], 49); // String containing the name of the block + uint8_t len = strlen(blocks[seq]); // Length of the block name + mavlink_msg_script_item_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid, + seq, len, block_name); + MAVLinkSendMessage(); + MAVLINK_DEBUG("Sent BLOCK_ITEM message: seq %i, name %s\n", seq, block_name); } else { - MAVLINK_DEBUG("ERROR: Block index out of bounds\n"); + MAVLINK_DEBUG("ERROR: Block index %i out of bounds\n", seq); } } -void mavlink_block_init(void) -{ -} - -void mavlink_block_cb(uint16_t current_block) -{ - mavlink_send_block(current_block); // send the current block seq - - mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, &timer_cb); // wait for ack -} - void mavlink_block_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { + /* request for script/block list, answer with number of blocks */ case MAVLINK_MSG_ID_SCRIPT_REQUEST_LIST: { MAVLINK_DEBUG("Received BLOCK_REQUEST_LIST message\n"); mavlink_script_request_list_t block_request_list_msg; - mavlink_msg_script_request_list_decode(msg, - &block_request_list_msg); // Cast the incoming message to a block_request_list_msg + mavlink_msg_script_request_list_decode(msg, &block_request_list_msg); if (block_request_list_msg.target_system == mavlink_system.sysid) { if (mission_mgr.state == STATE_IDLE) { if (NB_BLOCK > 0) { @@ -101,8 +79,8 @@ void mavlink_block_message_handler(const mavlink_message_t *msg) } mavlink_send_block_count(); - mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, - &timer_cb); // Register the timeout timer (it is continuous so it needs to be cancelled after triggering) + // Register the timeout timer (it is continuous so it needs to be cancelled after triggering) + mavlink_mission_set_timer(); } else { // TODO: Handle case when the state is not IDLE } @@ -113,16 +91,19 @@ void mavlink_block_message_handler(const mavlink_message_t *msg) break; } + /* request script/block, answer with SCRIPT_ITEM (block) */ case MAVLINK_MSG_ID_SCRIPT_REQUEST: { MAVLINK_DEBUG("Received BLOCK_REQUEST message\n"); mavlink_script_request_t block_request_msg; - mavlink_msg_script_request_decode(msg, &block_request_msg); // Cast the incoming message to a block_request_msg + mavlink_msg_script_request_decode(msg, &block_request_msg); if (block_request_msg.target_system == mavlink_system.sysid) { - // Handle only cases in which the entire list is request, the block is sent again, or the next block was sent + // Handle only cases in which the entire list is request, the block is sent again, + // or the next block was sent if ((mission_mgr.state == STATE_SEND_LIST && block_request_msg.seq == 0) || (mission_mgr.state == STATE_SEND_ITEM && (block_request_msg.seq == mission_mgr.seq || block_request_msg.seq == mission_mgr.seq + 1))) { - sys_time_cancel_timer(mission_mgr.timer_id); // Cancel the timeout timer + // Cancel the timeout timer + mavlink_mission_cancel_timer(); mission_mgr.state = STATE_SEND_ITEM; MAVLINK_DEBUG("State: %d\n", mission_mgr.state); @@ -130,7 +111,8 @@ void mavlink_block_message_handler(const mavlink_message_t *msg) mavlink_send_block(mission_mgr.seq); - mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, &timer_cb); // Register the timeout timer + // Register the timeout timer + mavlink_mission_set_timer(); } else { // TODO: Handle cases for which the above condition does not hold } @@ -141,15 +123,20 @@ void mavlink_block_message_handler(const mavlink_message_t *msg) break; } + /* got script item, change block */ case MAVLINK_MSG_ID_SCRIPT_ITEM: { MAVLINK_DEBUG("Received BLOCK_ITEM message\n"); mavlink_script_item_t block_item_msg; - mavlink_msg_script_item_decode(msg, &block_item_msg); // Cast the incoming message to a block_item_msg + mavlink_msg_script_item_decode(msg, &block_item_msg); if (block_item_msg.target_system == mavlink_system.sysid) { - if (mission_mgr.state == STATE_IDLE) { // Only handle incoming block item messages if there a not currently being sent + // Only handle incoming block item messages if they a not currently being sent + if (mission_mgr.state == STATE_IDLE) { nav_goto_block((uint8_t)block_item_msg.seq); // Set the current block } } } + + default: + break; } } diff --git a/sw/airborne/modules/datalink/missionlib/blocks.h b/sw/airborne/modules/datalink/missionlib/blocks.h index a23b89915a..2ec946c5ad 100644 --- a/sw/airborne/modules/datalink/missionlib/blocks.h +++ b/sw/airborne/modules/datalink/missionlib/blocks.h @@ -27,16 +27,9 @@ #ifndef MISSIONLIB_BLOCKS_H #define MISSIONLIB_BLOCKS_H -// Disable auto-data structures -#ifndef MAVLINK_NO_DATA -#define MAVLINK_NO_DATA -#endif +#include -#include "generated/flight_plan.h" -#include "mavlink/paparazzi/mavlink.h" - -extern void mavlink_block_init(void); -extern void mavlink_block_cb(uint16_t current_block); extern void mavlink_block_message_handler(const mavlink_message_t *msg); +extern void mavlink_send_block(uint16_t seq); #endif // MISSIONLIB_BLOCKS_H diff --git a/sw/airborne/modules/datalink/missionlib/mission_manager.c b/sw/airborne/modules/datalink/missionlib/mission_manager.c index 403f086372..0cc0d33379 100644 --- a/sw/airborne/modules/datalink/missionlib/mission_manager.c +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.c @@ -31,11 +31,45 @@ #include "modules/datalink/missionlib/blocks.h" #include "modules/datalink/missionlib/waypoints.h" -void mavlink_mission_init(mavlink_mission_mgr *mission_mgr) -{ - mavlink_wp_init(); +// include mavlink headers, but ignore some warnings +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wswitch-default" +#include "mavlink/paparazzi/mavlink.h" +#pragma GCC diagnostic pop - mission_mgr->seq = 0; +// for waypoints, include correct header until we have unified API +#ifdef AP +#include "subsystems/navigation/common_nav.h" +#else +#include "firmwares/rotorcraft/navigation.h" +#endif +#include "generated/flight_plan.h" + +#include "mcu_periph/sys_time.h" + + +void mavlink_mission_init(mavlink_mission_mgr *mgr) +{ + mgr->seq = 0; + mgr->timer_id = -1; +} + +void mavlink_mission_set_timer(void) +{ + if (mission_mgr.timer_id < 0) { + mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, NULL); + } + else { + sys_time_update_timer(mission_mgr.timer_id, MAVLINK_TIMEOUT); + } +} + +void mavlink_mission_cancel_timer(void) +{ + if (mission_mgr.timer_id >= 0) { + sys_time_cancel_timer(mission_mgr.timer_id); + } + mission_mgr.timer_id = -1; } void mavlink_mission_message_handler(const mavlink_message_t *msg) @@ -44,12 +78,37 @@ void mavlink_mission_message_handler(const mavlink_message_t *msg) mavlink_wp_message_handler(msg); - switch (msg->msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: { - MAVLINK_DEBUG("Received MISSION_ACK message\n"); - sys_time_cancel_timer(mission_mgr.timer_id); // Cancel the timeout timer - mission_mgr.state = STATE_IDLE; - MAVLINK_DEBUG("State: %d\n", mission_mgr.state); - } + if (msg->msgid == MAVLINK_MSG_ID_MISSION_ACK) { + MAVLINK_DEBUG("Received MISSION_ACK message\n"); + mavlink_mission_cancel_timer(); + mission_mgr.state = STATE_IDLE; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); } } + +/// update current block and send if changed +void mavlink_mission_periodic(void) +{ + // FIXME: really use the SCRIPT_ITEM message to indicate current block? + if (mission_mgr.current_block != nav_block) { + mission_mgr.current_block = nav_block; + mavlink_send_block(nav_block); // send the current block seq + // wait for ack, really? + mavlink_mission_set_timer(); + } + // check if we had a timeout on a transaction + if (sys_time_check_and_ack_timer(mission_mgr.timer_id)) { + mavlink_mission_cancel_timer(); + mission_mgr.state = STATE_IDLE; + mission_mgr.seq = 0; + MAVLINK_DEBUG("Warning: Mavlink mission request timed out!\n"); + } +} + +void mavlink_send_mission_ack(void) +{ + mavlink_msg_mission_ack_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid, + MAV_MISSION_ACCEPTED); + MAVLinkSendMessage(); + MAVLINK_DEBUG("Sent MISSION_ACK message\n"); +} diff --git a/sw/airborne/modules/datalink/missionlib/mission_manager.h b/sw/airborne/modules/datalink/missionlib/mission_manager.h index b19b221447..3c2e0de34d 100644 --- a/sw/airborne/modules/datalink/missionlib/mission_manager.h +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.h @@ -29,28 +29,21 @@ #ifndef MISSIONLIB_COMMON_H #define MISSIONLIB_COMMON_H -#include - -// #include "firmwares/rotorcraft/navigation.h" -#include "mcu_periph/sys_time.h" - -#include "generated/flight_plan.h" -#include "mavlink/paparazzi/mavlink.h" +#include #ifndef MAVLINK_TIMEOUT #define MAVLINK_TIMEOUT 15 // as in MAVLink waypoint convention #endif -// State machine +/// State machine enum MAVLINK_MISSION_MGR_STATES { STATE_IDLE = 0, STATE_SEND_LIST, STATE_SEND_ITEM, - STATE_WRITE_ITEM, // only for updating waypoints + STATE_WAYPOINT_WRITE_TRANSACTION }; struct mavlink_mission_mgr { - mavlink_mission_item_t waypoints[NB_WAYPOINT]; // Array containing the waypoints in global coordinates uint8_t current_block; // Counter that holds the index of the current block enum MAVLINK_MISSION_MGR_STATES state; // The current state of the mission handler uint16_t seq; // Sequence id (position of the current item on the list) @@ -63,29 +56,14 @@ typedef struct mavlink_mission_mgr mavlink_mission_mgr; extern mavlink_mission_mgr mission_mgr; -// Timer callback function -static inline void timer_cb(uint8_t id) -{ - sys_time_cancel_timer(id); // Cancel the timer that triggered the timeout event - mission_mgr.state = STATE_IDLE; - MAVLINK_DEBUG("ERROR: Request timed out!\n"); - // TODO: Handle timeout retries, for now just assume no retries -} -static inline void sendMissionAck() -{ - mavlink_message_t msg; - mavlink_mission_ack_t mission_ack; - mission_ack.target_system = mission_mgr.rem_sysid; - mission_ack.target_component = mission_mgr.rem_compid; - mission_ack.type = MAV_MISSION_ACCEPTED; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, - &mission_ack); // encode the ack message - MAVLINK_DEBUG("Sent MISSION_ACK message\n"); - mavlink_send_message(&msg); -} - -extern void mavlink_mission_init(mavlink_mission_mgr *mission_mgr); +extern void mavlink_mission_init(mavlink_mission_mgr *mgr); extern void mavlink_mission_message_handler(const mavlink_message_t *msg); +extern void mavlink_mission_periodic(void); + +extern void mavlink_send_mission_ack(void); + +extern void mavlink_mission_set_timer(void); +extern void mavlink_mission_cancel_timer(void); #endif diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.c b/sw/airborne/modules/datalink/missionlib/waypoints.c index 9acf40eb81..bff8adc123 100644 --- a/sw/airborne/modules/datalink/missionlib/waypoints.c +++ b/sw/airborne/modules/datalink/missionlib/waypoints.c @@ -28,112 +28,75 @@ // Include own header #include "modules/datalink/missionlib/waypoints.h" +#include "modules/datalink/missionlib/mission_manager.h" +#include "modules/datalink/mavlink.h" + +// include mavlink headers, but ignore some warnings +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wswitch-default" +#include "mavlink/paparazzi/mavlink.h" +#pragma GCC diagnostic pop + +#include "generated/flight_plan.h" -#include -#include #include "subsystems/navigation/waypoints.h" //#include "subsystems/navigation/common_nav.h" // for fixed-wing aircraft -#include "modules/datalink/mavlink.h" #include "modules/datalink/missionlib/mission_manager.h" -static void mavlink_update_wp_list(void) -{ - // Store the waypoints in a mission item - if (NB_WAYPOINT > 0) { -// for (uint8_t i = 0; i < NB_WAYPOINT; i++) { -// mavlink_mission_item_t mission_item; -// -// /* -// * Convert ENU waypoint to UTM -// * May be removed, but nav_move_waypoint() uses UTM coordinates in case of fixed-wing aircraft -// */ -// struct UtmCoor_f utm; -// utm.east = waypoints[i].x + nav_utm_east0; -// utm.north = waypoints[i].y + nav_utm_north0; -// utm.alt = waypoints[i].a; -// utm.zone = nav_utm_zone0; -// -// // Convert UTM waypoint to LLA -// struct LlaCoor_f lla; -// lla_of_utm_f(&lla, &utm); -// mission_item.x = lla.lat; // lattitude -// mission_item.y = lla.lon; // longtitude -// mission_item.z = lla.alt; // altitude -// mission_item.seq = i; -// -// mission_mgr.waypoints[i] = mission_item; -// } - for (uint8_t i = 0; i < NB_WAYPOINT; i++) { - mavlink_mission_item_t mission_item; - // waypoint_set_global_flag(i); - if (waypoint_is_global(i)) { - MAVLINK_DEBUG("Waypoint(%d): is global\n", i); - } else { - MAVLINK_DEBUG("Waypoint(%d): is NOT global\n", i); - } - waypoint_globalize(i); - mission_item.x = (float)waypoint_get_lla(i)->lat * 1e-7; // lattitude - mission_item.y = (float)waypoint_get_lla(i)->lon * 1e-7; // longtitude - mission_item.z = (float)waypoint_get_lla(i)->alt * 1e-3; // altitude - - MAVLINK_DEBUG("WP: %f, %f, %f\n", mission_item.x, mission_item.y, mission_item.z); - - mission_item.seq = i; - mission_mgr.waypoints[i] = mission_item; - } - } else { - MAVLINK_DEBUG("ERROR: The waypoint array is empty\n"); - } -} - -static void mavlink_send_wp_count(void) -{ - mavlink_message_t msg; - mavlink_mission_count_t wp_count; - wp_count.target_system = mission_mgr.rem_sysid; - wp_count.target_component = mission_mgr.rem_compid; - wp_count.count = NB_WAYPOINT; // From the generated flight plan - - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, - &wp_count); // encode the block count message - - MAVLINK_DEBUG("Sent WP_COUNT message\n"); - mavlink_send_message(&msg); -} - -static void mavlink_send_wp(uint16_t seq) +static void mavlink_send_wp(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < NB_WAYPOINT) { // Due to indexing - mavlink_message_t msg; - mavlink_mission_item_t *mission_item = &(mission_mgr.waypoints[seq]); // Copy the reference to the mission item - - mission_item->target_system = mission_mgr.rem_sysid; - mission_item->target_component = mission_mgr.rem_compid; - - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_system.compid, &msg, mission_item); - - MAVLINK_DEBUG("Sent MISSION_ITEM message\n"); - mavlink_send_message(&msg); +#ifdef AP + /* for fixedwing firmware send as LOCAL_ENU for now */ + mavlink_msg_mission_item_send(MAVLINK_COMM_0, + sysid, + compid, + seq, + MAV_FRAME_LOCAL_ENU, + MAV_CMD_NAV_WAYPOINT, + 0, // current + 0, // autocontinue + 0, 0, 0, 0, // params + WaypointX(seq), + WaypointY(seq), + WaypointAlt(seq)); +#else + /* for rotorcraft firmware use waypoint API and send as lat/lon */ + /* sending lat/lon as float is actually a bad idea, + * but it seems that most GCSs don't understand the MISSION_ITEM_INT + */ + struct LlaCoor_i *lla = waypoint_get_lla(seq); + mavlink_msg_mission_item_send(MAVLINK_COMM_0, + sysid, + compid, + seq, + MAV_FRAME_GLOBAL, + MAV_CMD_NAV_WAYPOINT, + 0, // current + 0, // autocontinue + 0, 0, 0, 0, // params + (float)lla->lat / 1e7, + (float)lla->lon / 1e7, + (float)lla->alt / 1e3); +#endif + MAVLinkSendMessage(); + MAVLINK_DEBUG("Sent MISSION_ITEM message with seq %i\n", seq); } else { - MAVLINK_DEBUG("ERROR: Wp index out of bounds\n"); + MAVLINK_DEBUG("ERROR: Wp index %i out of bounds\n", seq); } } -void mavlink_wp_init(void) -{ - mavlink_update_wp_list(); -} - void mavlink_wp_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { + + /* request for mission list, answer with number of waypoints */ case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { MAVLINK_DEBUG("Received MISSION_REQUEST_LIST message\n"); mavlink_mission_request_list_t mission_request_list_msg; - mavlink_msg_mission_request_list_decode(msg, - &mission_request_list_msg); // Cast the incoming message to a mission_request_list_msg + mavlink_msg_mission_request_list_decode(msg, &mission_request_list_msg); if (mission_request_list_msg.target_system == mavlink_system.sysid) { if (mission_mgr.state == STATE_IDLE) { if (NB_WAYPOINT > 0) { @@ -143,73 +106,163 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg) mission_mgr.rem_sysid = msg->sysid; mission_mgr.rem_compid = msg->compid; } - mavlink_send_wp_count(); + mavlink_msg_mission_count_send(MAVLINK_COMM_0, msg->sysid, msg->compid, NB_WAYPOINT); + MAVLinkSendMessage(); - mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, - &timer_cb); // Register the timeout timer (it is continuous so it needs to be cancelled after triggering) + // Register the timeout timer (it is continuous so it needs to be cancelled after triggering) + mavlink_mission_set_timer(); } else { // TODO: Handle case when the state is not IDLE } - } else { - // TODO: Handle remote system id mismatch } - break; } + /* request for mission item, answer with waypoint */ case MAVLINK_MSG_ID_MISSION_REQUEST: { - MAVLINK_DEBUG("Received MISSION_REQUEST message\n"); - mavlink_mission_request_t mission_request_msg; - mavlink_msg_mission_request_decode(msg, &mission_request_msg); // Cast the incoming message to a mission_request_msg - if (mission_request_msg.target_system == mavlink_system.sysid) { - if ((mission_mgr.state == STATE_SEND_LIST && mission_request_msg.seq == 0) || // Send the first waypoint - (mission_mgr.state == STATE_SEND_ITEM && (mission_request_msg.seq == mission_mgr.seq - || // Send the current waypoint again - mission_request_msg.seq == mission_mgr.seq + 1))) { // Send the next waypoint - sys_time_cancel_timer(mission_mgr.timer_id); // Cancel the timeout timer + mavlink_mission_request_t req; + mavlink_msg_mission_request_decode(msg, &req); + MAVLINK_DEBUG("Received MISSION_REQUEST message with seq %i\n", req.seq); - mission_mgr.state = STATE_SEND_ITEM; - MAVLINK_DEBUG("State: %d\n", mission_mgr.state); - mission_mgr.seq = mission_request_msg.seq; - - mavlink_update_wp_list(); // Update the waypoint list - - mavlink_send_wp(mission_mgr.seq); - - mission_mgr.timer_id = sys_time_register_timer(MAVLINK_TIMEOUT, &timer_cb); // Register the timeout timer - } else { - // TODO: Handle cases for which the above condition does not hold - } - } else { - // TODO: Handle remote system id mismatch + if (req.target_system != mavlink_system.sysid || req.seq >= NB_WAYPOINT) { + return; } + /* Send if: + * - the first waypoint + * - current waypoint requested again + * - or next waypoint requested + */ + if ((mission_mgr.state == STATE_SEND_LIST && req.seq == 0) || + (mission_mgr.state == STATE_SEND_ITEM && (req.seq == mission_mgr.seq || + req.seq == mission_mgr.seq + 1))) { + // Cancel the timeout timer + mavlink_mission_cancel_timer(); + mission_mgr.state = STATE_SEND_ITEM; + MAVLINK_DEBUG("State: %d\n", mission_mgr.state); + mission_mgr.seq = req.seq; + + mavlink_send_wp(msg->sysid, msg->compid, mission_mgr.seq); + + // Register the timeout timer + mavlink_mission_set_timer(); + } else { + // TODO: Handle cases for which the above condition does not hold + } break; } - case MAVLINK_MSG_ID_MISSION_ITEM: { - MAVLINK_DEBUG("Received MISSION_ITEM message\n"); - mavlink_mission_item_t mission_item_msg; - mavlink_msg_mission_item_decode(msg, &mission_item_msg); // Cast the incoming message to a mission_item_msg +#ifndef AP + /* change waypoints: only available when using waypoint API (rotorcraft firmware) + * This uses the waypoint_set_x functions (opposed to waypoint_move_x), + * meaning it doesn't send WP_MOVED Paparazzi messages. + */ - if (mission_item_msg.target_system == mavlink_system.sysid) { - if (mission_mgr.state == STATE_IDLE) { // Only handle incoming mission item messages if there a not currently being sent - struct LlaCoor_i lla; - lla.lat = (int32_t)(mission_item_msg.x * 1e7); // lattitude in degrees*1e7 - lla.lon = (int32_t)(mission_item_msg.y * 1e7); // longitude in degrees*1e7 - lla.alt = (int32_t)(mission_item_msg.z * 1e3); // altitude in millimeters - -// struct UtmCoor_f utm; -// utm.zone = nav_utm_zone0; -// utm_of_lla_f(&utm, &lla); - MAVLINK_DEBUG("Received WP(%d): %d, %d, %d\n", mission_item_msg.seq, lla.lat, lla.lon, lla.alt); - // Set the new waypoint - waypoint_move_lla(mission_item_msg.seq, &lla); // move the waypoint with the id equal to seq, only for rotorcraft -// nav_move_waypoint(mission_item_msg.seq, utm.east, utm.north, utm.alt); - - sendMissionAck(); - } + /* initiate mission/waypoint write transaction */ + case MAVLINK_MSG_ID_MISSION_COUNT: { + mavlink_mission_count_t mission_count; + mavlink_msg_mission_count_decode(msg, &mission_count); + if (mission_count.target_system != mavlink_system.sysid) { + return; } + MAVLINK_DEBUG("Received MISSION_COUNT message with count %i\n", mission_count.count); + /* only allow new waypoint update transaction if currently idle */ + if (mission_mgr.state != STATE_IDLE) { + MAVLINK_DEBUG("MISSION_COUNT error: mission manager not idle.\n"); + return; + } + if (mission_count.count != NB_WAYPOINT) { + MAVLINK_DEBUG("MISSION_COUNT error: request writing %i instead of %i waypoints\n", + mission_count.count, NB_WAYPOINT); + return; + } + /* valid initiation of waypoint write transaction, ask for first waypoint */ + MAVLINK_DEBUG("MISSION_COUNT: Requesting first waypoint\n"); + mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, 0); + MAVLinkSendMessage(); + + mission_mgr.seq = 0; + mission_mgr.state = STATE_WAYPOINT_WRITE_TRANSACTION; + + // Register the timeout timer + mavlink_mission_set_timer(); } + break; + + /* got MISSION_ITEM, update one waypoint if in valid transaction */ + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t mission_item; + mavlink_msg_mission_item_decode(msg, &mission_item); + + if (mission_item.target_system != mavlink_system.sysid) { + return; + } + + MAVLINK_DEBUG("Received MISSION_ITEM message with seq %i and frame %i\n", + mission_item.seq, mission_item.frame); + + /* reject non waypoint updates */ + if (mission_item.command != MAV_CMD_NAV_WAYPOINT || + mission_item.seq >= NB_WAYPOINT) { + MAVLINK_DEBUG("rejected MISSION_ITEM command %i, seq %i\n", + mission_item.command, mission_item.seq); + return; + } + + /* Only handle mission item if correct sequence */ + if (mission_mgr.state != STATE_WAYPOINT_WRITE_TRANSACTION) { + MAVLINK_DEBUG("got MISSION_ITEM while not in waypoint write transaction\n"); + // Do we want to handle updating waypoints outside of full transaction? + } + if (mission_item.seq != mission_mgr.seq) { + MAVLINK_DEBUG("MISSION_ITEM, got waypoint seq %i, but requested %i\n", + mission_item.seq, mission_mgr.seq); + return; + } + if (mission_item.frame == MAV_FRAME_GLOBAL) { + MAVLINK_DEBUG("MISSION_ITEM, global wp: lat=%f, lon=%f, alt=%f\n", + mission_item.x, mission_item.y, mission_item.z); + struct LlaCoor_i lla; + lla.lat = mission_item.x * 1e7; // lattitude in degrees*1e7 + lla.lon = mission_item.y * 1e7; // longitude in degrees*1e7 + lla.alt = mission_item.z * 1e3; // altitude in millimeters + waypoint_set_lla(mission_item.seq, &lla); + } + else if (mission_item.frame == MAV_FRAME_LOCAL_ENU) { + MAVLINK_DEBUG("MISSION_ITEM, local_enu wp: x=%f, y=%f, z=%f\n", + mission_item.x, mission_item.y, mission_item.z); + struct EnuCoor_f enu; + enu.x = mission_item.x; + enu.y = mission_item.y; + enu.z = mission_item.z; + waypoint_set_enu(mission_item.seq, &enu); + } + else { + MAVLINK_DEBUG("No handler for MISSION_ITEM with frame %i\n", mission_item.frame); + return; + } + // acknowledge transmission of all waypoints or request next waypoint + if (mission_item.seq == NB_WAYPOINT -1) { + MAVLINK_DEBUG("Acknowledging end of waypoint write transaction\n"); + mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, + MAV_MISSION_ACCEPTED); + MAVLinkSendMessage(); + mavlink_mission_cancel_timer(); + mission_mgr.state = STATE_IDLE; + } + else { + MAVLINK_DEBUG("Requesting waypoint %i\n", mission_item.seq + 1); + mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, + mission_item.seq + 1); + MAVLinkSendMessage(); + mission_mgr.seq = mission_item.seq + 1; + mavlink_mission_set_timer(); + } + break; + } +#endif // AP + + default: + break; } } diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.h b/sw/airborne/modules/datalink/missionlib/waypoints.h index 84c2ca2125..a355a9d95a 100644 --- a/sw/airborne/modules/datalink/missionlib/waypoints.h +++ b/sw/airborne/modules/datalink/missionlib/waypoints.h @@ -20,23 +20,15 @@ * */ +/** @file modules/datalink/missionlib/waypoints.h + * + */ + #ifndef MISSIONLIB_WAYPOINTS_H #define MISSIONLIB_WAYPOINTS_H -// Disable auto-data structures -#ifndef MAVLINK_NO_DATA -#define MAVLINK_NO_DATA -#endif +#include -#include "generated/flight_plan.h" -#include "mavlink/paparazzi/mavlink.h" - -// Block storage struct -#ifndef MAVLINK_MAX_WP_COUNT -#define MAVLINK_MAX_WP_COUNT NB_WAYPOINT -#endif - -extern void mavlink_wp_init(void); extern void mavlink_wp_message_handler(const mavlink_message_t *msg); -#endif // MISSIONLIB_WAYPOINTS_H \ No newline at end of file +#endif // MISSIONLIB_WAYPOINTS_H From 5fcd8dc4802d4704775cf3fd52f01746ee840db4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 16 Nov 2015 13:56:47 +0100 Subject: [PATCH 09/12] [mavlink] don't wait for ack when sending current nav_block --- sw/airborne/modules/datalink/missionlib/mission_manager.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/sw/airborne/modules/datalink/missionlib/mission_manager.c b/sw/airborne/modules/datalink/missionlib/mission_manager.c index 0cc0d33379..6b79f5cb74 100644 --- a/sw/airborne/modules/datalink/missionlib/mission_manager.c +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.c @@ -93,8 +93,6 @@ void mavlink_mission_periodic(void) if (mission_mgr.current_block != nav_block) { mission_mgr.current_block = nav_block; mavlink_send_block(nav_block); // send the current block seq - // wait for ack, really? - mavlink_mission_set_timer(); } // check if we had a timeout on a transaction if (sys_time_check_and_ack_timer(mission_mgr.timer_id)) { From e5816299e763b74c9825b5d629622c60a4554f73 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 16 Nov 2015 13:56:57 +0100 Subject: [PATCH 10/12] [mavlink] add SYSTEM_TIME message --- sw/airborne/modules/datalink/mavlink.c | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 0c9ac233b8..b00da9cc65 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -65,6 +65,7 @@ void mavlink_common_message_handler(const mavlink_message_t *msg); static inline void mavlink_send_heartbeat(void); static inline void mavlink_send_sys_status(void); +static inline void mavlink_send_system_time(void); static inline void mavlink_send_attitude(void); static inline void mavlink_send_local_position_ned(void); static inline void mavlink_send_global_position_int(void); @@ -103,6 +104,7 @@ void mavlink_periodic(void) { RunOnceEvery(2, mavlink_send_heartbeat()); RunOnceEvery(5, mavlink_send_sys_status()); + RunOnceEvery(20, mavlink_send_system_time()); RunOnceEvery(10, mavlink_send_attitude()); RunOnceEvery(5, mavlink_send_attitude_quaternion()); RunOnceEvery(5, mavlink_send_params()); @@ -354,6 +356,17 @@ static inline void mavlink_send_sys_status(void) MAVLinkSendMessage(); } +/** + * 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(); +} + /** * Send the attitude */ From 2babaf6331b3337e0fad695c840b6f0725a559d1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 16 Nov 2015 21:39:52 +0100 Subject: [PATCH 11/12] [mavlink] allow single waypoint update if a MISSION_ITEM is sent from the GCS without a MISSION_COUNT first, then simply directly update that waypoint and answer with MISSION_ACK --- .../modules/datalink/missionlib/waypoints.c | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.c b/sw/airborne/modules/datalink/missionlib/waypoints.c index bff8adc123..823d63227e 100644 --- a/sw/airborne/modules/datalink/missionlib/waypoints.c +++ b/sw/airborne/modules/datalink/missionlib/waypoints.c @@ -209,16 +209,18 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg) return; } - /* Only handle mission item if correct sequence */ - if (mission_mgr.state != STATE_WAYPOINT_WRITE_TRANSACTION) { - MAVLINK_DEBUG("got MISSION_ITEM while not in waypoint write transaction\n"); - // Do we want to handle updating waypoints outside of full transaction? + /* Only accept mission item in IDLE (update single waypoint) or in write transaction */ + if (mission_mgr.state != STATE_IDLE && mission_mgr.state != STATE_WAYPOINT_WRITE_TRANSACTION) { + MAVLINK_DEBUG("got MISSION_ITEM while not in waypoint write transaction or idle\n"); + return; } - if (mission_item.seq != mission_mgr.seq) { + /* If in write transaction, only handle mission item if correct sequence */ + if (mission_mgr.state == STATE_WAYPOINT_WRITE_TRANSACTION && mission_item.seq != mission_mgr.seq) { MAVLINK_DEBUG("MISSION_ITEM, got waypoint seq %i, but requested %i\n", mission_item.seq, mission_mgr.seq); return; } + if (mission_item.frame == MAV_FRAME_GLOBAL) { MAVLINK_DEBUG("MISSION_ITEM, global wp: lat=%f, lon=%f, alt=%f\n", mission_item.x, mission_item.y, mission_item.z); @@ -241,8 +243,16 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg) MAVLINK_DEBUG("No handler for MISSION_ITEM with frame %i\n", mission_item.frame); return; } - // acknowledge transmission of all waypoints or request next waypoint - if (mission_item.seq == NB_WAYPOINT -1) { + // acknowledge transmission single waypoint update + if (mission_mgr.state == STATE_IDLE) { + MAVLINK_DEBUG("Acknowledge single waypoint update\n"); + mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, + MAV_MISSION_ACCEPTED); + MAVLinkSendMessage(); + mavlink_mission_cancel_timer(); + } + // or end of transaction + else if (mission_item.seq == NB_WAYPOINT -1) { MAVLINK_DEBUG("Acknowledging end of waypoint write transaction\n"); mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); @@ -250,6 +260,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg) mavlink_mission_cancel_timer(); mission_mgr.state = STATE_IDLE; } + // or request next waypoint if still in middle of transaction else { MAVLINK_DEBUG("Requesting waypoint %i\n", mission_item.seq + 1); mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, From b75f5d15e105e4815407c3a704d84d836b184af8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 16 Nov 2015 21:45:29 +0100 Subject: [PATCH 12/12] [mavlink] update waypoint on MISSION_ITEM_INT if frame is GLOBAL_INT --- .../modules/datalink/missionlib/waypoints.c | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.c b/sw/airborne/modules/datalink/missionlib/waypoints.c index 823d63227e..38be0ee63f 100644 --- a/sw/airborne/modules/datalink/missionlib/waypoints.c +++ b/sw/airborne/modules/datalink/missionlib/waypoints.c @@ -271,6 +271,36 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg) } break; } + + case MAVLINK_MSG_ID_MISSION_ITEM_INT: { + mavlink_mission_item_int_t mission_item; + mavlink_msg_mission_item_int_decode(msg, &mission_item); + + if (mission_item.target_system == mavlink_system.sysid) { + MAVLINK_DEBUG("Received MISSION_ITEM_INT message with seq %i and frame %i\n", + mission_item.seq, mission_item.frame); + if (mission_item.seq >= NB_WAYPOINT) { + return; + } + if (mission_item.frame == MAV_FRAME_GLOBAL_INT) { + MAVLINK_DEBUG("MISSION_ITEM_INT, global_int wp: lat=%i, lon=%i, alt=%f\n", + mission_item.x, mission_item.y, mission_item.z); + struct LlaCoor_i lla; + lla.lat = mission_item.x; // lattitude in degrees*1e7 + lla.lon = mission_item.y; // longitude in degrees*1e7 + lla.alt = mission_item.z * 1e3; // altitude in millimeters + waypoint_set_lla(mission_item.seq, &lla); + mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, + MAV_MISSION_ACCEPTED); + MAVLinkSendMessage(); + } + else { + MAVLINK_DEBUG("No handler for MISSION_ITEM_INT with frame %i\n", mission_item.frame); + return; + } + } + } + break; #endif // AP default: