Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: use Location::AltFrame for guided_state.target_alt_frame

This commit is contained in:
timtuxworth 2024-08-24 16:52:16 -06:00 committed by Peter Barker
parent a4753f32ac
commit 1133f82799
5 changed files with 43 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduPlane/GCS_Mavlink.cpp
View File

@ -932,48 +932,25 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
return MAV_RESULT_DENIED; return MAV_RESULT_DENIED;
} }
// the requested alt data might be relative or absolute Location::AltFrame new_target_alt_frame;
float new_target_alt = packet.z * 100; if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, new_target_alt_frame)) {
float new_target_alt_rel = packet.z * 100 + plane.home.alt; return MAV_RESULT_DENIED;
// only global/relative/terrain frames are supported
switch(packet.frame) {
case MAV_FRAME_GLOBAL_RELATIVE_ALT: {
if (is_equal(plane.guided_state.target_alt,new_target_alt_rel) ) { // compare two floats as near-enough
// no need to process any new packet/s with the same ALT any further, if we are already doing it.
return MAV_RESULT_ACCEPTED;
}
plane.guided_state.target_alt = new_target_alt_rel;
break;
}
case MAV_FRAME_GLOBAL: {
if (is_equal(plane.guided_state.target_alt,new_target_alt) ) { // compare two floats as near-enough
// no need to process any new packet/s with the same ALT any further, if we are already doing it.
return MAV_RESULT_ACCEPTED;
}
plane.guided_state.target_alt = new_target_alt;
break;
}
default:
// MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters).
return MAV_RESULT_DENIED;
} }
// keep a copy of what came in via MAVLink - this is needed for logging, but not for anything else
plane.guided_state.target_mav_frame = packet.frame;
plane.guided_state.target_alt_frame = packet.frame; const int32_t new_target_alt_cm = packet.z * 100;
plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here plane.guided_state.target_location.set_alt_cm(new_target_alt_cm, new_target_alt_frame);
plane.guided_state.target_alt_time_ms = AP_HAL::millis(); plane.guided_state.target_alt_time_ms = AP_HAL::millis();
// param3 contains the desired vertical velocity (not acceleration)
if (is_zero(packet.param3)) { if (is_zero(packet.param3)) {
// the user wanted /maximum acceleration, pick a large value as close enough // the user wanted /maximum altitude change rate, pick a large value as close enough
plane.guided_state.target_alt_accel = 1000.0; plane.guided_state.target_alt_rate = 1000.0;
} else { } else {
plane.guided_state.target_alt_accel = fabsf(packet.param3); plane.guided_state.target_alt_rate = fabsf(packet.param3);
} }
// assign an acceleration direction
if (plane.guided_state.target_alt < plane.current_loc.alt) {
plane.guided_state.target_alt_accel *= -1.0f;
}
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }

23
ArduPlane/Log.cpp
View File

@ -128,10 +128,11 @@ struct PACKED log_OFG_Guided {
float target_airspeed_cm; float target_airspeed_cm;
float target_airspeed_accel; float target_airspeed_accel;
float target_alt; float target_alt;
float target_alt_accel; float target_alt_rate;
uint8_t target_alt_frame; uint8_t target_mav_frame; // received MavLink frame
float target_heading; float target_heading;
float target_heading_limit; float target_heading_limit;
uint8_t target_alt_frame; // internal AltFrame
}; };
// Write a OFG Guided packet. // Write a OFG Guided packet.
@ -142,11 +143,12 @@ void Plane::Log_Write_OFG_Guided()
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
target_airspeed_cm : (float)guided_state.target_airspeed_cm*(float)0.01, target_airspeed_cm : (float)guided_state.target_airspeed_cm*(float)0.01,
target_airspeed_accel : guided_state.target_airspeed_accel, target_airspeed_accel : guided_state.target_airspeed_accel,
target_alt : guided_state.target_alt, target_alt : guided_state.target_location.alt * 0.01,
target_alt_accel : guided_state.target_alt_accel, target_alt_rate : guided_state.target_alt_rate,
target_alt_frame : guided_state.target_alt_frame, target_mav_frame : guided_state.target_mav_frame,
target_heading : guided_state.target_heading, target_heading : guided_state.target_heading,
target_heading_limit : guided_state.target_heading_accel_limit target_heading_limit : guided_state.target_heading_accel_limit,
target_alt_frame : static_cast<uint8_t>(guided_state.target_location.get_alt_frame()),
}; };
logger.WriteBlock(&pkt, sizeof(pkt)); logger.WriteBlock(&pkt, sizeof(pkt));
} }
@ -274,7 +276,7 @@ void Plane::Log_Write_Guided(void)
logger.Write_PID(LOG_PIDG_MSG, g2.guidedHeading.get_pid_info()); logger.Write_PID(LOG_PIDG_MSG, g2.guidedHeading.get_pid_info());
} }
if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) { if ( guided_state.target_location.alt != -1 || is_positive(guided_state.target_airspeed_cm) ) {
Log_Write_OFG_Guided(); Log_Write_OFG_Guided();
} }
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
@ -490,12 +492,13 @@ const struct LogStructure Plane::log_structure[] = {
// @Field: Arsp: target airspeed cm // @Field: Arsp: target airspeed cm
// @Field: ArspA: target airspeed accel // @Field: ArspA: target airspeed accel
// @Field: Alt: target alt // @Field: Alt: target alt
// @Field: AltA: target alt accel // @Field: AltA: target alt velocity (rate of change)
// @Field: AltF: target alt frame // @Field: AltF: target alt frame (MAVLink)
// @Field: Hdg: target heading // @Field: Hdg: target heading
// @Field: HdgA: target heading lim // @Field: HdgA: target heading lim
// @Field: AltL: target alt frame (Location)
{ LOG_OFG_MSG, sizeof(log_OFG_Guided), { LOG_OFG_MSG, sizeof(log_OFG_Guided),
"OFG", "QffffBff", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, "OFG", "QffffBffB", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true },
#endif #endif
}; };

7
ArduPlane/Plane.h
View File

@ -577,11 +577,10 @@ private:
uint32_t target_airspeed_time_ms; uint32_t target_airspeed_time_ms;
// altitude adjustments // altitude adjustments
float target_alt = -1; // don't default to zero here, as zero is a valid alt. Location target_location;
uint32_t last_target_alt = 0; float target_alt_rate;
float target_alt_accel;
uint32_t target_alt_time_ms = 0; uint32_t target_alt_time_ms = 0;
uint8_t target_alt_frame = 0; uint8_t target_mav_frame = -1;
// heading track // heading track
float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians

3
ArduPlane/mode.cpp
View File

@ -58,9 +58,8 @@ bool Mode::enter()
plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
plane.guided_state.target_alt_time_ms = 0; plane.guided_state.target_alt_time_ms = 0;
plane.guided_state.last_target_alt = 0; plane.guided_state.target_location.set_alt_cm(-1, Location::AltFrame::ABSOLUTE);
#endif #endif
#if AP_CAMERA_ENABLED #if AP_CAMERA_ENABLED

25
ArduPlane/mode_guided.cpp
View File

@ -129,24 +129,29 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi
void ModeGuided::update_target_altitude() void ModeGuided::update_target_altitude()
{ {
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero. // target altitude can be negative (e.g. flying below home altitude from the top of a mountain)
if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_location.alt != -1 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
// offboard altitude demanded // offboard altitude demanded
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms); float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms);
plane.guided_state.target_alt_time_ms = now; plane.guided_state.target_alt_time_ms = now;
// determine delta accurately as a float // determine delta accurately as a float
float delta_amt_f = delta * plane.guided_state.target_alt_accel; float delta_amt_f = delta * plane.guided_state.target_alt_rate;
// then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative // then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f); int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f);
Location temp {}; // To calculate the required velocity (up or down), we need to target and current altitudes in the target frame
temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here, const Location::AltFrame target_frame = plane.guided_state.target_location.get_alt_frame();
if (is_positive(plane.guided_state.target_alt_accel)) { int32_t target_alt_previous_cm;
temp.alt = MIN(plane.guided_state.target_alt, temp.alt); if (plane.current_loc.initialised() && plane.guided_state.target_location.initialised() &&
} else { plane.current_loc.get_alt_cm(target_frame, target_alt_previous_cm)) {
temp.alt = MAX(plane.guided_state.target_alt, temp.alt); // create a new interim target location that that takes current_location and moves delta_amt_i in the right direction
int32_t temp_alt_cm = constrain_int32(plane.guided_state.target_location.alt, target_alt_previous_cm - delta_amt_i, target_alt_previous_cm + delta_amt_i);
Location temp_location = plane.guided_state.target_location;
temp_location.set_alt_cm(temp_alt_cm, target_frame);
// incrementally step the altitude towards the target
plane.set_target_altitude_location(temp_location);
} }
plane.guided_state.last_target_alt = temp.alt;
plane.set_target_altitude_location(temp);
} else } else
#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
{ {