Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: support *10 multipler when storing/retrieving radius in NAV_LOITER_TURNS

Browse Source
This commit is contained in:
Peter Barker 2022-05-11 13:40:48 +10:00 committed by Randy Mackay
parent 913b789592
commit d435181756
2 changed files with 27 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Mission/AP_Mission.cpp
View File

@ -640,6 +640,7 @@ struct PACKED Packed_Location_Option_Flags {
uint8_t terrain_alt : 1; // this altitude is above terrain uint8_t terrain_alt : 1; // this altitude is above terrain
uint8_t origin_alt : 1; // this altitude is above ekf origin 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 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 { 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.alt = packed_content.location.alt;
cmd.content.location.lat = packed_content.location.lat; cmd.content.location.lat = packed_content.location.lat;
cmd.content.location.lng = packed_content.location.lng; cmd.content.location.lng = packed_content.location.lng;
if (packed_content.location.flags.type_specific_bit_0) {
cmd.type_specific_bits = 1U << 0;
}
} else { } else {
// all other options in Content are assumed to be packed: // all other options in Content are assumed to be packed:
static_assert(sizeof(cmd.content) >= 12, 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.terrain_alt = cmd.content.location.terrain_alt;
packed.location.flags.origin_alt = cmd.content.location.origin_alt; packed.location.flags.origin_alt = cmd.content.location.origin_alt;
packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack; 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.alt = cmd.content.location.alt;
packed.location.lat = cmd.content.location.lat; packed.location.lat = cmd.content.location.lat;
packed.location.lng = cmd.content.location.lng; 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; break;
case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18 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 // number of turns is stored in the lowest bits. radii below
uint16_t radius_m = MIN(255,fabsf(packet.param3)); // param 3 is radius in meters is held in high p1 // 255m are stored in the top 8 bits as an 8-bit integer.
cmd.p1 = (radius_m<<8) | (num_turns & 0x00FF); // store radius in high byte of p1, num turns in low byte of p1 // 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_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 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) { if (cmd.content.location.loiter_ccw) {
packet.param3 = -packet.param3; 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 packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
break; break;

4
libraries/AP_Mission/AP_Mission.h
View File

@ -313,6 +313,10 @@ public:
uint16_t p1; // general purpose parameter 1 uint16_t p1; // general purpose parameter 1
Content content; 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 // return a human-readable interpretation of the ID stored in this command
const char *type() const; const char *type() const;