Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #1422 from paparazzi/mavlink_additions

Browse Source 
Mavlink additions

- fix sending of some mavlink messages (MAVLinkSendMessage() was missing)
- send GPS_STATUS and VFR_HUD and SYSTEM_TIME
- waypoint protocol:
  - parse MISSION_ITEM_INT message to update single waypoints
  - only for rotorcraft (since it used the waypoint interface, see https://github.com/paparazzi/paparazzi/issues/981)
  - start waypoint write/update transaction upon receiving MISSION_COUNT message (with requesting of waypoints)
  - send ack only after all waypoints are transferred
- add the Paparazzi specific SCRIPT_ITEM messages

Updating waypoints seems to work fine with e.g. QGroundControl now.
This commit is contained in:
Felix Ruess
2015-11-17 23:24:21 +01:00
parent a48df1dfe1 b75f5d15e1
commit b69aa2d7bf
10 changed files with 892 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/mavlink.xml
+3
View File
@@ -16,6 +16,9 @@
<makefile>
<file name="mavlink.c"/>
<file name="mission_manager.c" dir="modules/datalink/missionlib"/>
<file name="waypoints.c" dir="modules/datalink/missionlib"/>
<file name="blocks.c" dir="modules/datalink/missionlib"/>
<raw>
MAVLINK_PORT ?= UART1
MAVLINK_PORT_UPPER=$(shell echo $(MAVLINK_PORT) | tr a-z A-Z)
sw/airborne/modules/datalink/mavlink.c
+178 -121
View File
@@ -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 <stdint.h>
@@ -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();
}
sw/airborne/modules/datalink/mavlink.h
+8
View File
@@ -40,6 +40,14 @@
#endif
#include "mcu_periph/uart.h"
#ifndef MAVLINK_DEBUG
#define MAVLINK_DEBUG(...) {}
#endif
#if MAVLINK_DEBUG == printf
#include <stdio.h>
#endif
/*
* MAVLink description before main MAVLink include
*/
sw/airborne/modules/datalink/missionlib/blocks.c
+142
View File
@@ -0,0 +1,142 @@
/*
* Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
*
* 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;
}
}
sw/airborne/modules/datalink/missionlib/blocks.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
*
* 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 <mavlink/mavlink_types.h>
extern void mavlink_block_message_handler(const mavlink_message_t *msg);
extern void mavlink_send_block(uint16_t seq);
#endif // MISSIONLIB_BLOCKS_H
sw/airborne/modules/datalink/missionlib/mission_manager.c
+112
View File
@@ -0,0 +1,112 @@
/*
* Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
*
* 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");
}
sw/airborne/modules/datalink/missionlib/mission_manager.h
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
*
* 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 <mavlink/mavlink_types.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_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
sw/airborne/modules/datalink/missionlib/waypoints.c
+309
View File
@@ -0,0 +1,309 @@
/*
* Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
*
* 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;
}
}
sw/airborne/modules/datalink/missionlib/waypoints.h
+34
View File
@@ -0,0 +1,34 @@
/*
* Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
*
* 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 <mavlink/mavlink_types.h>
extern void mavlink_wp_message_handler(const mavlink_message_t *msg);
#endif // MISSIONLIB_WAYPOINTS_H
sw/tools/generators/gen_flight_plan.ml
+2 -2
View File
@@ -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);