From 40267706cc353c71e75c316c6fa8c4bfa0f68b6d Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Sat, 24 Aug 2024 16:52:16 -0600 Subject: [PATCH] ArduPlane: use Location::AltFrame for guided_state.target_alt_frame --- ArduPlane/GCS_Mavlink.cpp | 51 +++++++++++++++++++-------------------- ArduPlane/Log.cpp | 13 ++++++---- ArduPlane/Plane.h | 4 ++- ArduPlane/mode.cpp | 1 + ArduPlane/mode_guided.cpp | 10 +++++--- 5 files changed, 44 insertions(+), 35 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 887599d6b985db..d321c096b53ec0 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -929,35 +929,33 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl 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; - float new_target_alt_rel = packet.z * 100 + plane.home.alt; - - // 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; + if (mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, new_target_alt_frame)) { + + // the requested alt data might be relative (to home) + if (new_target_alt_frame == Location::AltFrame::ABOVE_HOME) { + new_target_alt = packet.z * 100 + plane.home.alt; } - 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; + + // compare two floats as near-enough + if (is_equal(plane.guided_state.target_alt,new_target_alt) && plane.guided_state.target_alt_frame == new_target_alt_frame) { + // no need to process any new packet/s with the same ALT any further, if we are already doing it. + return MAV_RESULT_ACCEPTED; } - default: - // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). - return MAV_RESULT_DENIED; } + else{ + return MAV_RESULT_DENIED; + } + + plane.guided_state.last_target_alt = plane.current_loc.alt; + plane.guided_state.last_target_alt_frame = plane.current_loc.get_alt_frame(); + + plane.guided_state.target_mav_frame = packet.frame; + plane.guided_state.target_alt_frame = new_target_alt_frame; + plane.guided_state.target_alt = new_target_alt; - plane.guided_state.target_alt_frame = packet.frame; - plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here plane.guided_state.target_alt_time_ms = AP_HAL::millis(); if (is_zero(packet.param3)) { @@ -967,8 +965,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl plane.guided_state.target_alt_accel = fabsf(packet.param3); } - // assign an acceleration direction - if (plane.guided_state.target_alt < plane.current_loc.alt) { + // assign an acceleration direction - this only works if the current and target frame are the same + // this should not be happening here - it shoudl be in ModeGuided::update_target_altitude() + if (plane.guided_state.last_target_alt_frame == plane.guided_state.target_alt_frame && plane.guided_state.target_alt < plane.current_loc.alt) { plane.guided_state.target_alt_accel *= -1.0f; } return MAV_RESULT_ACCEPTED; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c9fdd0783b767c..90a75c9b199f2a 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -129,9 +129,10 @@ struct PACKED log_OFG_Guided { float target_airspeed_accel; float target_alt; float target_alt_accel; - uint8_t target_alt_frame; + uint8_t target_mav_frame; // received MavLink frame float target_heading; float target_heading_limit; + uint8_t target_alt_frame; // internal AltFrame }; // Write a OFG Guided packet. @@ -144,9 +145,10 @@ void Plane::Log_Write_OFG_Guided() target_airspeed_accel : guided_state.target_airspeed_accel, target_alt : guided_state.target_alt, target_alt_accel : guided_state.target_alt_accel, - target_alt_frame : guided_state.target_alt_frame, + target_mav_frame : guided_state.target_mav_frame, 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(guided_state.target_alt_frame), }; logger.WriteBlock(&pkt, sizeof(pkt)); } @@ -491,11 +493,12 @@ const struct LogStructure Plane::log_structure[] = { // @Field: ArspA: target airspeed accel // @Field: Alt: target alt // @Field: AltA: target alt accel -// @Field: AltF: target alt frame +// @Field: AltF: target alt frame (MAVLink) // @Field: Hdg: target heading // @Field: HdgA: target heading lim +// @Field: AltL: target alt frame (Location) { 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 }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..b8bbfc7293ab5b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -563,9 +563,11 @@ class Plane : public AP_Vehicle { // altitude adjustments float target_alt = -1; // don't default to zero here, as zero is a valid alt. uint32_t last_target_alt = 0; + Location::AltFrame last_target_alt_frame = Location::AltFrame::ABSOLUTE; float target_alt_accel; uint32_t target_alt_time_ms = 0; - uint8_t target_alt_frame = 0; + Location::AltFrame target_alt_frame = Location::AltFrame::ABSOLUTE; + uint8_t target_mav_frame = -1; // heading track float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a74d7c1a9367a0..ad7a25e778a4df 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -59,6 +59,7 @@ bool Mode::enter() 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_alt = -1; // same as above, although a target alt of -1 is rare on plane. + plane.guided_state.target_alt_frame = Location::AltFrame::ABSOLUTE; plane.guided_state.target_alt_time_ms = 0; plane.guided_state.last_target_alt = 0; #endif diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 4fef88ee3c73f4..5602ec9b5cbe50 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -139,13 +139,17 @@ void ModeGuided::update_target_altitude() // 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); Location temp {}; - temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here, + int32_t temp_alt = plane.guided_state.last_target_alt + delta_amt_i; + // Restore the last altitude that the plane had been flying if (is_positive(plane.guided_state.target_alt_accel)) { - temp.alt = MIN(plane.guided_state.target_alt, temp.alt); + temp_alt = MIN(plane.guided_state.target_alt, temp_alt); } else { - temp.alt = MAX(plane.guided_state.target_alt, temp.alt); + temp_alt = MAX(plane.guided_state.target_alt, temp_alt); } + temp.set_alt_cm(temp_alt, plane.guided_state.last_target_alt_frame); + plane.guided_state.last_target_alt = temp.alt; + plane.guided_state.last_target_alt_frame = temp.get_alt_frame(); plane.set_target_altitude_location(temp); } else #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED