Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 08:55:51 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[mavlink] Update to protocol V2

Browse Source
This commit is contained in:
Freek van Tienen
2023-05-23 15:58:06 +02:00
parent 2a5c6d3eec
commit ff415a62bd
6 changed files with 67 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/telemetry/highspeed_rotorcraft.xml
+18
View File
@@ -245,5 +245,23 @@
</mode> </mode>
</process> </process>
<process name="Mavlink" type="mavlink">
<mode name="default">
<message name="HEARTBEAT" period="0.5"/> <!-- For the camera 10Hz -->
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="SYS_STATUS" period="1.2"/>
<message name="SYSTEM_TIME" period="2.0"/>
<message name="ATTITUDE" period="0.1"/> <!-- For the camera 10Hz -->
<message name="ATTITUDE_QUATERNION" period="1.5"/>
<message name="LOCAL_POSITION_NED" period="0.8"/>
<message name="GLOBAL_POSITION_INT" period="0.1"/> <!-- For the camera 10Hz -->
<message name="GPS_RAW_INT" period="0.5"/> <!-- For the camera 2Hz -->
<message name="BATTERY_STATUS" period="4.0"/>
<message name="GPS_GLOBAL_ORIGIN" period="5.1"/>
<message name="VFR_HUD" period="0.5"/>
<message name="RC_CHANNELS" period="0.2"/>
</mode>
</process>
</telemetry> </telemetry>
sw/airborne/modules/datalink/mavlink.c
+34 -14
View File
@@ -28,6 +28,7 @@
// include mavlink headers, but ignore some warnings // include mavlink headers, but ignore some warnings
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default" #pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h" #include "mavlink/paparazzi/mavlink.h"
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
@@ -107,7 +108,7 @@ struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlin
void mavlink_init(void) void mavlink_init(void)
{ {
mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255 mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
mavlink_system.compid = MAV_COMP_ID_MISSIONPLANNER; // Component/Subsystem ID, 1-255 mavlink_system.compid = MAV_COMP_ID_AUTOPILOT1; // Component/Subsystem ID, 1-255
get_pprz_git_version(custom_version); get_pprz_git_version(custom_version);
@@ -352,7 +353,7 @@ void mavlink_common_message_handler(const mavlink_message_t *msg)
break; break;
} }
// confirm command with result // confirm command with result
mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result); mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result, 0, UINT8_MAX, msg->sysid, msg->compid);
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
break; break;
@@ -527,7 +528,10 @@ static void mavlink_send_sys_status(struct transport_tx *trans, struct link_devi
0, // Autopilot specific error 1 0, // Autopilot specific error 1
0, // Autopilot specific error 2 0, // Autopilot specific error 2
0, // Autopilot specific error 3 0, // Autopilot specific error 3
0); // Autopilot specific error 4 0, // Autopilot specific error 4
0,
0,
0);
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
@@ -578,13 +582,14 @@ static void mavlink_send_global_position_int(struct transport_tx *trans, struct
heading += 360; heading += 360;
} }
uint16_t compass_heading = heading * 100; uint16_t compass_heading = heading * 100;
int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl; int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_i.lla.alt;
int32_t hmsl_alt = state.ned_origin_i.hmsl - state.ned_origin_i.lla.alt;
/// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs /// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs
mavlink_msg_global_position_int_send(MAVLINK_COMM_0, mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
get_sys_time_msec(), get_sys_time_msec(),
stateGetPositionLla_i()->lat, stateGetPositionLla_i()->lat,
stateGetPositionLla_i()->lon, stateGetPositionLla_i()->lon,
stateGetPositionLla_i()->alt, stateGetPositionLla_i()->alt + hmsl_alt,
relative_alt, relative_alt,
stateGetSpeedNed_f()->x * 100, stateGetSpeedNed_f()->x * 100,
stateGetSpeedNed_f()->y * 100, stateGetSpeedNed_f()->y * 100,
@@ -599,7 +604,8 @@ static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct li
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
state.ned_origin_i.lla.lat, state.ned_origin_i.lla.lat,
state.ned_origin_i.lla.lon, state.ned_origin_i.lla.lon,
state.ned_origin_i.hmsl); state.ned_origin_i.hmsl,
get_sys_time_usec());
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
} }
@@ -641,13 +647,14 @@ static void mavlink_send_autopilot_version(struct transport_tx *trans, struct li
0, //const uint8_t *os_custom_version, 0, //const uint8_t *os_custom_version,
0, //uint16_t vendor_id, 0, //uint16_t vendor_id,
0, //uint16_t product_id, 0, //uint16_t product_id,
sha //uint64_t uid sha, //uint64_t uid
); 0);
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev) static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
{ {
float repr_offset_q[4] = {0, 0, 0, 0};
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
get_sys_time_msec(), get_sys_time_msec(),
stateGetNedToBodyQuat_f()->qi, stateGetNedToBodyQuat_f()->qi,
@@ -656,7 +663,8 @@ static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct
stateGetNedToBodyQuat_f()->qz, stateGetNedToBodyQuat_f()->qz,
stateGetBodyRates_f()->p, stateGetBodyRates_f()->p,
stateGetBodyRates_f()->q, stateGetBodyRates_f()->q,
stateGetBodyRates_f()->r); stateGetBodyRates_f()->r,
repr_offset_q);
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
@@ -676,12 +684,18 @@ static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_dev
gps.fix, gps.fix,
gps.lla_pos.lat, gps.lla_pos.lat,
gps.lla_pos.lon, gps.lla_pos.lon,
gps.lla_pos.alt, gps.hmsl,
gps.pdop, gps.pdop,
UINT16_MAX, // VDOP UINT16_MAX, // VDOP
gps.gspeed, gps.gspeed,
course, course,
gps.num_sv); gps.num_sv,
gps.lla_pos.alt,
gps.hacc,
gps.vacc,
gps.sacc,
0,
0);
MAVLinkSendMessage(); MAVLinkSendMessage();
#endif #endif
} }
@@ -771,7 +785,7 @@ static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_dev
#include "modules/energy/electrical.h" #include "modules/energy/electrical.h"
static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev) static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev)
{ {
static uint16_t voltages[10]; static uint16_t voltages[14] = {0};
// we simply only set one cell for now // we simply only set one cell for now
voltages[0] = electrical.vsupply * 1000.f; // convert to mV voltages[0] = electrical.vsupply * 1000.f; // convert to mV
/// TODO: check what all these fields are supposed to represent /// TODO: check what all these fields are supposed to represent
@@ -784,7 +798,12 @@ static void mavlink_send_battery_status(struct transport_tx *trans, struct link_
electrical.current * 100.f, // convert to deciA electrical.current * 100.f, // convert to deciA
electrical.charge * 1000.f, // convert to mAh electrical.charge * 1000.f, // convert to mAh
electrical.energy * 36, // convert to hecto Joule electrical.energy * 36, // convert to hecto Joule
-1); // remaining percentage not estimated -1, // remaining percentage not estimated
0,
MAV_BATTERY_CHARGE_STATE_UNDEFINED,
&voltages[10],
MAV_BATTERY_MODE_UNKNOWN,
0);
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
@@ -804,12 +823,13 @@ static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device
#elif defined COMMAND_THROTTLE #elif defined COMMAND_THROTTLE
throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100); throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
#endif #endif
float hmsl_alt = state.ned_origin_f.hmsl - state.ned_origin_f.lla.alt;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
stateGetAirspeed_f(), stateGetAirspeed_f(),
stateGetHorizontalSpeedNorm_f(), // groundspeed stateGetHorizontalSpeedNorm_f(), // groundspeed
heading, heading,
throttle, throttle,
stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL stateGetPositionLla_f()->alt + hmsl_alt,
stateGetSpeedNed_f()->z); // climb rate stateGetSpeedNed_f()->z); // climb rate
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
sw/airborne/modules/datalink/missionlib/blocks.c
+1
View File
@@ -30,6 +30,7 @@
// include mavlink headers, but ignore some warnings // include mavlink headers, but ignore some warnings
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default" #pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h" #include "mavlink/paparazzi/mavlink.h"
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
sw/airborne/modules/datalink/missionlib/mission_manager.c
+2 -1
View File
@@ -33,6 +33,7 @@
// include mavlink headers, but ignore some warnings // include mavlink headers, but ignore some warnings
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default" #pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h" #include "mavlink/paparazzi/mavlink.h"
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
@@ -109,7 +110,7 @@ void mavlink_mission_periodic(void)
void mavlink_send_mission_ack(void) void mavlink_send_mission_ack(void)
{ {
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid, mavlink_msg_mission_ack_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid,
MAV_MISSION_ACCEPTED); MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
MAVLINK_DEBUG("Sent MISSION_ACK message\n"); MAVLINK_DEBUG("Sent MISSION_ACK message\n");
} }
sw/airborne/modules/datalink/missionlib/waypoints.c
+11 -8
View File
@@ -33,6 +33,7 @@
// include mavlink headers, but ignore some warnings // include mavlink headers, but ignore some warnings
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default" #pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h" #include "mavlink/paparazzi/mavlink.h"
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
@@ -61,7 +62,8 @@ static void mavlink_send_wp(uint8_t sysid, uint8_t compid, uint16_t seq)
0, 0, 0, 0, // params 0, 0, 0, 0, // params
WaypointX(seq), WaypointX(seq),
WaypointY(seq), WaypointY(seq),
WaypointAlt(seq)); WaypointAlt(seq),
MAV_MISSION_TYPE_MISSION);
#else #else
/* for rotorcraft firmware use waypoint API and send as lat/lon */ /* for rotorcraft firmware use waypoint API and send as lat/lon */
/* sending lat/lon as float is actually a bad idea, /* sending lat/lon as float is actually a bad idea,
@@ -79,7 +81,8 @@ static void mavlink_send_wp(uint8_t sysid, uint8_t compid, uint16_t seq)
0, 0, 0, 0, // params 0, 0, 0, 0, // params
(float)lla->lat / 1e7, (float)lla->lat / 1e7,
(float)lla->lon / 1e7, (float)lla->lon / 1e7,
(float)lla->alt / 1e3); (float)lla->alt / 1e3,
MAV_MISSION_TYPE_MISSION);
#endif #endif
MAVLinkSendMessage(); MAVLinkSendMessage();
MAVLINK_DEBUG("Sent MISSION_ITEM message with seq %i\n", seq); MAVLINK_DEBUG("Sent MISSION_ITEM message with seq %i\n", seq);
@@ -106,7 +109,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
mission_mgr.rem_sysid = msg->sysid; mission_mgr.rem_sysid = msg->sysid;
mission_mgr.rem_compid = msg->compid; mission_mgr.rem_compid = msg->compid;
} }
mavlink_msg_mission_count_send(MAVLINK_COMM_0, msg->sysid, msg->compid, NB_WAYPOINT); mavlink_msg_mission_count_send(MAVLINK_COMM_0, msg->sysid, msg->compid, NB_WAYPOINT, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
// 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)
@@ -178,7 +181,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
} }
/* valid initiation of waypoint write transaction, ask for first waypoint */ /* valid initiation of waypoint write transaction, ask for first waypoint */
MAVLINK_DEBUG("MISSION_COUNT: Requesting first waypoint\n"); MAVLINK_DEBUG("MISSION_COUNT: Requesting first waypoint\n");
mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, 0); mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, 0, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
mission_mgr.seq = 0; mission_mgr.seq = 0;
@@ -256,7 +259,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
if (mission_mgr.state == STATE_IDLE) { if (mission_mgr.state == STATE_IDLE) {
MAVLINK_DEBUG("Acknowledge single waypoint update\n"); MAVLINK_DEBUG("Acknowledge single waypoint update\n");
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
MAV_MISSION_ACCEPTED); MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
mavlink_mission_cancel_timer(); mavlink_mission_cancel_timer();
} }
@@ -264,7 +267,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
else if (mission_item.seq == NB_WAYPOINT -1) { else if (mission_item.seq == NB_WAYPOINT -1) {
MAVLINK_DEBUG("Acknowledging end of waypoint write transaction\n"); MAVLINK_DEBUG("Acknowledging end of waypoint write transaction\n");
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
MAV_MISSION_ACCEPTED); MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
mavlink_mission_cancel_timer(); mavlink_mission_cancel_timer();
mission_mgr.state = STATE_IDLE; mission_mgr.state = STATE_IDLE;
@@ -273,7 +276,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
else { else {
MAVLINK_DEBUG("Requesting waypoint %i\n", mission_item.seq + 1); MAVLINK_DEBUG("Requesting waypoint %i\n", mission_item.seq + 1);
mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
mission_item.seq + 1); mission_item.seq + 1, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
mission_mgr.seq = mission_item.seq + 1; mission_mgr.seq = mission_item.seq + 1;
mavlink_mission_set_timer(); mavlink_mission_set_timer();
@@ -300,7 +303,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
lla.alt = mission_item.z * 1e3; // altitude in millimeters lla.alt = mission_item.z * 1e3; // altitude in millimeters
waypoint_set_lla(mission_item.seq, &lla); waypoint_set_lla(mission_item.seq, &lla);
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid, mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
MAV_MISSION_ACCEPTED); MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage(); MAVLinkSendMessage();
} }
else { else {
sw/ext/Makefile
+1 -1
View File
@@ -91,7 +91,7 @@ mavlink: mavlink.update mavlink.build
mavlink.build: mavlink.build:
@echo GENERATE $(PAPARAZZI_SRC)/var/include/mavlink @echo GENERATE $(PAPARAZZI_SRC)/var/include/mavlink
$(Q)PYTHONPATH=$(EXT_DIR)/mavlink python $(EXT_DIR)/mavlink/pymavlink/tools/mavgen.py --output $(PAPARAZZI_SRC)/var/include/mavlink --lang C $(EXT_DIR)/mavlink/message_definitions/v1.0/paparazzi.xml --no-validate > /dev/null $(Q)PYTHONPATH=$(EXT_DIR)/mavlink python $(EXT_DIR)/mavlink/pymavlink/tools/mavgen.py --output $(PAPARAZZI_SRC)/var/include/mavlink --lang C $(EXT_DIR)/mavlink/message_definitions/v1.0/paparazzi.xml --wire-protocol 2.0 --no-validate > /dev/null
libsbp: libsbp.update libsbp: libsbp.update