From aa59e0a63eba32d34f8a3b70cc3b1621b8e3bbe0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 11 May 2022 13:40:48 +1000 Subject: [PATCH] AP_Mission: support *10 multipler when storing/retrieving radius in NAV_LOITER_TURNS --- libraries/AP_Mission/AP_Mission.cpp | 26 +++++++++++++++++++++++--- libraries/AP_Mission/AP_Mission.h | 4 ++++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 97e0c321b3..dac4f78642 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -640,6 +640,7 @@ struct PACKED Packed_Location_Option_Flags { uint8_t terrain_alt : 1; // this altitude is above terrain uint8_t origin_alt : 1; // this altitude is above ekf origin uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location + uint8_t type_specific_bit_0 : 1; // each mission item type can use this for storing 1 bit of extra data }; struct PACKED PackedLocation { @@ -725,6 +726,10 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con cmd.content.location.alt = packed_content.location.alt; cmd.content.location.lat = packed_content.location.lat; cmd.content.location.lng = packed_content.location.lng; + + if (packed_content.location.flags.type_specific_bit_0) { + cmd.type_specific_bits = 1U << 0; + } } else { // all other options in Content are assumed to be packed: static_assert(sizeof(cmd.content) >= 12, @@ -786,6 +791,7 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd packed.location.flags.terrain_alt = cmd.content.location.terrain_alt; packed.location.flags.origin_alt = cmd.content.location.origin_alt; packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack; + packed.location.flags.type_specific_bit_0 = cmd.type_specific_bits & (1U<<0); packed.location.alt = cmd.content.location.alt; packed.location.lat = cmd.content.location.lat; packed.location.lng = cmd.content.location.lng; @@ -921,9 +927,20 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18 - uint16_t num_turns = MIN(255,packet.param1); // param 1 is number of times to circle is held in low p1 - uint16_t radius_m = MIN(255,fabsf(packet.param3)); // param 3 is radius in meters is held in high p1 - cmd.p1 = (radius_m<<8) | (num_turns & 0x00FF); // store radius in high byte of p1, num turns in low byte of p1 + // number of turns is stored in the lowest bits. radii below + // 255m are stored in the top 8 bits as an 8-bit integer. + // Radii above 255m are stored divided by 10 and a bit set in + // storage so that on retrieval they are multiplied by 10 + cmd.p1 = MIN(255, packet.param1); // store number of times to circle in low p1 + uint8_t radius_m; + const float abs_radius = fabsf(packet.param3); + if (abs_radius <= 255) { + radius_m = abs_radius; + } else { + radius_m = MIN(255, abs_radius * 0.1); + cmd.type_specific_bits = 1U << 0; + } + cmd.p1 |= (radius_m<<8); // store radius in high byte of p1 cmd.content.location.loiter_ccw = (packet.param3 < 0); cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location } @@ -1389,6 +1406,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c if (cmd.content.location.loiter_ccw) { packet.param3 = -packet.param3; } + if (cmd.type_specific_bits & (1U<<0)) { + packet.param3 *= 10; + } packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 04f2bf16aa..a246c3a676 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -313,6 +313,10 @@ public: uint16_t p1; // general purpose parameter 1 Content content; + // for items which store in location, we offer a few more bits + // of storage: + uint8_t type_specific_bits; // bitmask of set/unset bits + // return a human-readable interpretation of the ID stored in this command const char *type() const;