ArduPlane: use Location::AltFrame for guided_state.target_alt_frame

ArduPilot · Aug 26, 2024 · 3d151ab · 3d151ab
1 parent c56439b
commit 3d151ab
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 6 deletions.
diff --git 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,14 +950,14 @@ 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:
                 //  MAV_RESULT_DENIED  means Command is invalid (is supported but has invalid parameters).
                 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
@@ -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<uint8_t>(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
@@ -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
@@ -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
@@ -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