diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 15ca3ab9e1..f88f7dbca6 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -728,6 +728,7 @@ struct PACKED Packed_Location_Option_Flags { 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 + uint8_t type_specific_bit_1 : 1; // each mission item type can use this for storing 1 bit of extra data }; struct PACKED PackedLocation { @@ -815,7 +816,10 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con cmd.content.location.lng = packed_content.location.lng; if (packed_content.location.flags.type_specific_bit_0) { - cmd.type_specific_bits = 1U << 0; + cmd.type_specific_bits |= 1U << 0; + } + if (packed_content.location.flags.type_specific_bit_1) { + cmd.type_specific_bits |= 1U << 1; } } else { // all other options in Content are assumed to be packed: @@ -882,7 +886,8 @@ 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.flags.type_specific_bit_0 = (cmd.type_specific_bits & (1U<<0)) >> 0; + packed.location.flags.type_specific_bit_1 = (cmd.type_specific_bits & (1U<<1)) >> 1; packed.location.alt = cmd.content.location.alt; packed.location.lat = cmd.content.location.lat; packed.location.lng = cmd.content.location.lng; @@ -1029,18 +1034,25 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18 - // number of turns is stored in the lowest bits. radii below - // 255m are stored in the top 8 bits as an 8-bit integer. + // number of turns is stored in the lowest bits. Number of + // turns 0 < N < 1 are stored multiplied by 256 and a bit set + // in storage so that on retrieval they are divided by 256. + // 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 + float param1_stored = packet.param1; + if (param1_stored > 0 && param1_stored < 1) { + param1_stored *= 256.0; + cmd.type_specific_bits |= (1U << 1); + } + cmd.p1 = MIN(255, param1_stored); // 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.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); @@ -1550,6 +1562,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c if (cmd.type_specific_bits & (1U<<0)) { packet.param3 *= 10; } + if (cmd.type_specific_bits & (1U<<1)) { + packet.param1 /= 256.0; + } 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 b50fa4d982..c7204b315b 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -415,6 +415,19 @@ public: // comparison operator (relies on all bytes in the structure even if they may not be used) bool operator ==(const Mission_Command &b) const { return (memcmp(this, &b, sizeof(Mission_Command)) == 0); } bool operator !=(const Mission_Command &b) const { return !operator==(b); } + + /* + return the number of turns for a LOITER_TURNS command + this has special handling for loiter turns from cmd.p1 and type_specific_bits + */ + float get_loiter_turns(void) const { + float turns = LOWBYTE(p1); + if (type_specific_bits & (1U<<1)) { + // special storage handling allows for fractional turns + turns *= (1.0/256.0); + } + return turns; + } };