Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 02:53:15 +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>
</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>
sw/airborne/modules/datalink/mavlink.c
+34 -14
View File
@@ -28,6 +28,7 @@
// include mavlink headers, but ignore some warnings
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h"
#pragma GCC diagnostic pop
@@ -107,7 +108,7 @@ struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlin
void mavlink_init(void)
{
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);
@@ -352,7 +353,7 @@ void mavlink_common_message_handler(const mavlink_message_t *msg)
break;
}
// 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();
}
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 2
0, // Autopilot specific error 3
0); // Autopilot specific error 4
0, // Autopilot specific error 4
0,
0,
0);
MAVLinkSendMessage();
}
@@ -578,13 +582,14 @@ static void mavlink_send_global_position_int(struct transport_tx *trans, struct
heading += 360;
}
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
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionLla_i()->lat,
stateGetPositionLla_i()->lon,
stateGetPositionLla_i()->alt,
stateGetPositionLla_i()->alt + hmsl_alt,
relative_alt,
stateGetSpeedNed_f()->x * 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,
state.ned_origin_i.lla.lat,
state.ned_origin_i.lla.lon,
state.ned_origin_i.hmsl);
state.ned_origin_i.hmsl,
get_sys_time_usec());
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, //uint16_t vendor_id,
0, //uint16_t product_id,
sha //uint64_t uid
);
sha, //uint64_t uid
0);
MAVLinkSendMessage();
}
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,
get_sys_time_msec(),
stateGetNedToBodyQuat_f()->qi,
@@ -656,7 +663,8 @@ static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct
stateGetNedToBodyQuat_f()->qz,
stateGetBodyRates_f()->p,
stateGetBodyRates_f()->q,
stateGetBodyRates_f()->r);
stateGetBodyRates_f()->r,
repr_offset_q);
MAVLinkSendMessage();
}
@@ -676,12 +684,18 @@ static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_dev
gps.fix,
gps.lla_pos.lat,
gps.lla_pos.lon,
gps.lla_pos.alt,
gps.hmsl,
gps.pdop,
UINT16_MAX, // VDOP
gps.gspeed,
course,
gps.num_sv);
gps.num_sv,
gps.lla_pos.alt,
gps.hacc,
gps.vacc,
gps.sacc,
0,
0);
MAVLinkSendMessage();
#endif
}
@@ -771,7 +785,7 @@ static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_dev
#include "modules/energy/electrical.h"
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
voltages[0] = electrical.vsupply * 1000.f; // convert to mV
/// 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.charge * 1000.f, // convert to mAh
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();
}
@@ -804,12 +823,13 @@ static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device
#elif defined COMMAND_THROTTLE
throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
#endif
float hmsl_alt = state.ned_origin_f.hmsl - state.ned_origin_f.lla.alt;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
stateGetAirspeed_f(),
stateGetHorizontalSpeedNorm_f(), // groundspeed
heading,
throttle,
stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
stateGetPositionLla_f()->alt + hmsl_alt,
stateGetSpeedNed_f()->z); // climb rate
MAVLinkSendMessage();
}
sw/airborne/modules/datalink/missionlib/blocks.c
+1
View File
@@ -30,6 +30,7 @@
// include mavlink headers, but ignore some warnings
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h"
#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
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h"
#pragma GCC diagnostic pop
@@ -109,7 +110,7 @@ void mavlink_mission_periodic(void)
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);
MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage();
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
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wswitch-default"
#include "mavlink/paparazzi/mavlink.h"
#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
WaypointX(seq),
WaypointY(seq),
WaypointAlt(seq));
WaypointAlt(seq),
MAV_MISSION_TYPE_MISSION);
#else
/* for rotorcraft firmware use waypoint API and send as lat/lon */
/* 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
(float)lla->lat / 1e7,
(float)lla->lon / 1e7,
(float)lla->alt / 1e3);
(float)lla->alt / 1e3,
MAV_MISSION_TYPE_MISSION);
#endif
MAVLinkSendMessage();
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_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();
// 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 */
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();
mission_mgr.seq = 0;
@@ -256,7 +259,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
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);
MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage();
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) {
MAVLINK_DEBUG("Acknowledging end of waypoint write transaction\n");
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
MAV_MISSION_ACCEPTED);
MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage();
mavlink_mission_cancel_timer();
mission_mgr.state = STATE_IDLE;
@@ -273,7 +276,7 @@ void mavlink_wp_message_handler(const mavlink_message_t *msg)
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);
mission_item.seq + 1, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage();
mission_mgr.seq = mission_item.seq + 1;
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
waypoint_set_lla(mission_item.seq, &lla);
mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
MAV_MISSION_ACCEPTED);
MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
MAVLinkSendMessage();
}
else {
sw/ext/Makefile
+1 -1
View File
@@ -91,7 +91,7 @@ mavlink: mavlink.update mavlink.build
mavlink.build:
@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