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: fractional Loiter Turn Support

Browse Source 
Adds special storage handling for loiter turns. Fractional Loiter
Turns 0<N<1 are stored by multiplying the turn number by 256, then
dividing that number by 256 on retrieval.
This commit is contained in:
J.R. Bronkar 2024-01-12 15:14:19 +11:00 committed by Tom Pittenger
parent 3a3f4ea446
commit 784a21bcab
2 changed files with 34 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Mission/AP_Mission.cpp
View File

@ -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;

13
libraries/AP_Mission/AP_Mission.h
View File

@ -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;
}
};