diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 887599d6b985db..3c39dee70dbbc4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -941,6 +941,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl return MAV_RESULT_ACCEPTED; } plane.guided_state.target_alt = new_target_alt_rel; + plane.guided_state.target_alt_frame = Location::AltFrame::ABOVE_HOME; break; } case MAV_FRAME_GLOBAL: { @@ -949,6 +950,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl return MAV_RESULT_ACCEPTED; } plane.guided_state.target_alt = new_target_alt; + plane.guided_state.target_alt_frame = Location::AltFrame::ABSOLUTE; break; } default: @@ -956,7 +958,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl return MAV_RESULT_DENIED; } - 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(); diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c9fdd0783b767c..3562bb56a0ac48 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -144,7 +144,7 @@ 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_alt_frame : static_cast(guided_state.target_alt_frame), target_heading : guided_state.target_heading, target_heading_limit : guided_state.target_heading_accel_limit }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..3ed4f13b21afe0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -563,9 +563,10 @@ 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; // 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