ArduPlane: use Location::AltFrame for guided_state.target_alt_frame

ArduPilot · Aug 30, 2024 · 258af30 · 258af30
1 parent 6d844d8
commit 258af30
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 10 deletions.
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -934,13 +934,15 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
         float new_target_alt_rel = packet.z * 100 + plane.home.alt;
 
          // only global/relative/terrain frames are supported
+        plane.guided_state.target_mav_frame = packet.frame;
         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;
+                plane.guided_state.target_alt_frame = Location::AltFrame::ABOVE_HOME;
                 break;
             }
             case MAV_FRAME_GLOBAL: {
@@ -949,14 +951,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
@@ -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<uint8_t>(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
@@ -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
@@ -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