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 c8a6e62864..b00da9cc65 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,8 +59,13 @@ 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 */ +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_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); @@ -77,6 +76,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 @@ -91,6 +92,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); } /** @@ -101,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()); @@ -112,6 +116,10 @@ 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()); + + mavlink_mission_periodic(); } static int16_t settings_idx_from_param_id(char *param_id) @@ -153,139 +161,104 @@ 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; + mavlink_common_message_handler(&msg); + mavlink_mission_message_handler(&msg); + } + } +} - /* 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); +void mavlink_common_message_handler(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + break; - mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); - MAVLinkSendMessage(); - 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); - /* 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); + 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); + 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; - } + 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 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); + /* 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); - } + // 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(); + 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; - } + break; + } - case MAVLINK_MSG_ID_PARAM_SET: { - mavlink_param_set_t set; - mavlink_msg_param_set_decode(&msg, &set); + 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); + // 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); + // 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 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) { - 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)); - MAVLinkSendMessage(); - } - } - } - break; - - default: - //Do nothing - //printf("Received message with id: %d\r\n", msg.msgid); - break; } } + break; + default: + //Do nothing + //MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); + break; } } @@ -383,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 */ @@ -484,6 +468,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 +482,7 @@ static inline void mavlink_send_attitude_quaternion(void) stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r); + MAVLinkSendMessage(); } #if USE_GPS @@ -521,6 +507,49 @@ static inline void mavlink_send_gps_raw_int(void) gps.gspeed, course, gps.num_sv); + MAVLinkSendMessage(); +#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 } @@ -562,6 +591,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 +611,31 @@ static inline void mavlink_send_battery_status(void) electrical.consumed, electrical.energy, // check scaling -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(); } 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 new file mode 100644 index 0000000000..1f264cb18f --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/blocks.c @@ -0,0 +1,142 @@ +/* + * 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 "modules/datalink/missionlib/blocks.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_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); +} + +void mavlink_send_block(uint16_t seq) +{ + if (seq < NB_BLOCK) { // Due to indexing + 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 %i out of bounds\n", seq); + } +} + +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); + 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(); + + // 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 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); + 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))) { + // 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 = block_request_msg.seq; + + mavlink_send_block(mission_mgr.seq); + + // Register the timeout timer + mavlink_mission_set_timer(); + } else { + // TODO: Handle cases for which the above condition does not hold + } + } else { + // TODO: Handle remote system id mismatch + } + + 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); + if (block_item_msg.target_system == mavlink_system.sysid) { + // 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 new file mode 100644 index 0000000000..2ec946c5ad --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/blocks.h @@ -0,0 +1,35 @@ +/* + * 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 + +#include + +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 new file mode 100644 index 0000000000..6b79f5cb74 --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.c @@ -0,0 +1,112 @@ +/* + * 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" + +// 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 + +// 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) +{ + mavlink_block_message_handler(msg); + + mavlink_wp_message_handler(msg); + + 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 + } + // 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 new file mode 100644 index 0000000000..3c2e0de34d --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.h @@ -0,0 +1,69 @@ +/* + * 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 + +#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_WAYPOINT_WRITE_TRANSACTION +}; + +struct mavlink_mission_mgr { + 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; + + +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 new file mode 100644 index 0000000000..38be0ee63f --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/waypoints.c @@ -0,0 +1,309 @@ +/* + * 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 "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 "subsystems/navigation/waypoints.h" +//#include "subsystems/navigation/common_nav.h" // for fixed-wing aircraft + +#include "modules/datalink/missionlib/mission_manager.h" + +static void mavlink_send_wp(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < NB_WAYPOINT) { // Due to indexing +#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 %i out of bounds\n", seq); + } +} + +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); + 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_msg_mission_count_send(MAVLINK_COMM_0, msg->sysid, msg->compid, NB_WAYPOINT); + MAVLinkSendMessage(); + + // 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 + } + } + 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); + MAVLINK_DEBUG("Received MISSION_REQUEST message with seq %i\n", req.seq); + + 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; + } + +#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. + */ + + /* 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 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 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); + 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 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); + MAVLinkSendMessage(); + 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, + mission_item.seq + 1); + MAVLinkSendMessage(); + mission_mgr.seq = mission_item.seq + 1; + mavlink_mission_set_timer(); + } + 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: + break; + } +} diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.h b/sw/airborne/modules/datalink/missionlib/waypoints.h new file mode 100644 index 0000000000..a355a9d95a --- /dev/null +++ b/sw/airborne/modules/datalink/missionlib/waypoints.h @@ -0,0 +1,34 @@ +/* + * 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.h + * + */ + +#ifndef MISSIONLIB_WAYPOINTS_H +#define MISSIONLIB_WAYPOINTS_H + +#include + +extern void mavlink_wp_message_handler(const mavlink_message_t *msg); + +#endif // MISSIONLIB_WAYPOINTS_H 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);