diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml index bf8faa94b0..5a023a4148 100644 --- a/conf/telemetry/highspeed_rotorcraft.xml +++ b/conf/telemetry/highspeed_rotorcraft.xml @@ -245,5 +245,23 @@ + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 7b850359ed..6d58471bbc 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -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(); } diff --git a/sw/airborne/modules/datalink/missionlib/blocks.c b/sw/airborne/modules/datalink/missionlib/blocks.c index 4cf929cbe8..fd075af574 100644 --- a/sw/airborne/modules/datalink/missionlib/blocks.c +++ b/sw/airborne/modules/datalink/missionlib/blocks.c @@ -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 diff --git a/sw/airborne/modules/datalink/missionlib/mission_manager.c b/sw/airborne/modules/datalink/missionlib/mission_manager.c index 5dca0710f6..e980db1f3f 100644 --- a/sw/airborne/modules/datalink/missionlib/mission_manager.c +++ b/sw/airborne/modules/datalink/missionlib/mission_manager.c @@ -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"); } diff --git a/sw/airborne/modules/datalink/missionlib/waypoints.c b/sw/airborne/modules/datalink/missionlib/waypoints.c index b824a96fdd..3d8b4506b1 100644 --- a/sw/airborne/modules/datalink/missionlib/waypoints.c +++ b/sw/airborne/modules/datalink/missionlib/waypoints.c @@ -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 { diff --git a/sw/ext/Makefile b/sw/ext/Makefile index e8062e203c..2f6eb29999 100644 --- a/sw/ext/Makefile +++ b/sw/ext/Makefile @@ -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