Sub: log alt_frame in GUIP

ArduSub/Log.cpp

@@ -178,6 +178,7 @@ struct PACKED log_GuidedTarget {
     LOG_PACKET_HEADER;
     uint64_t time_us;
     uint8_t type;
+    uint8_t alt_frame;
     float pos_target_x;
     float pos_target_y;
     float pos_target_z;
@@ -187,12 +188,14 @@ struct PACKED log_GuidedTarget {
 };
 
 // Write a Guided mode target
-void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
+void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target,
+                                 Location::AltFrame alt_frame)
 {
     struct log_GuidedTarget pkt = {
         LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
         time_us         : AP_HAL::micros64(),
         type            : target_type,
+        alt_frame       : static_cast<uint8_t>(alt_frame),
         pos_target_x    : pos_target.x,
         pos_target_y    : pos_target.y,
         pos_target_z    : pos_target.z,
@@ -253,6 +256,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target
 // @Description: Guided mode target information
 // @Field: TimeUS: Time since system startup
 // @Field: Type: Type of guided mode
+// @Field: Frame: Location.AltFrame
 // @Field: pX: Target position, X-Axis
 // @Field: pY: Target position, Y-Axis
 // @Field: pZ: Target position, Z-Axis
@@ -278,7 +282,7 @@ const struct LogStructure Sub::log_structure[] = {
     { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),         
       "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--" },
     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
-      "GUIP",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
+      "GUIP",  "QBBffffff",    "TimeUS,Type,Frame,pX,pY,pZ,vX,vY,vZ", "s--mmmnnn", "F--000000" },
 };
 
 uint8_t Sub::get_num_log_structures() const

ArduSub/Sub.h

@@ -422,7 +422,8 @@ class Sub : public AP_Vehicle {
     void Log_Write_Data(LogDataID id, int16_t value);
     void Log_Write_Data(LogDataID id, uint16_t value);
     void Log_Write_Data(LogDataID id, float value);
-    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
+    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target,
+                                Location::AltFrame alt_frame=Location::AltFrame::ABOVE_ORIGIN);
     void Log_Write_Vehicle_Startup_Messages();
 #endif
     void load_parameters(void) override;

ArduSub/mode.h

@@ -326,7 +326,7 @@ class ModeGuided : public Mode
     void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
     void guided_set_angle(const Quaternion&, float);
     void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
-    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame frame=Location::AltFrame::ABOVE_ORIGIN);
+    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame alt_frame=Location::AltFrame::ABOVE_ORIGIN);
     bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
     bool guided_set_destination(const Vector3f& destination);
     bool guided_set_destination(const Location&);

ArduSub/mode_guided.cpp

@@ -296,7 +296,7 @@ void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, flo
 }
 
 // set guided mode posvel target
-bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame frame)
+bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame alt_frame)
 {
     // check we are in velocity control mode
     if (sub.guided_mode != Guided_PosVel) {
@@ -305,7 +305,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
 
 #if AP_FENCE_ENABLED
     // reject destination if outside the fence
-    const Location dest_loc(destination, frame);
+    const Location dest_loc(destination, alt_frame);
     if (!sub.fence.check_destination_within_fence(dest_loc)) {
         LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
@@ -315,7 +315,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
 
 #if AP_RANGEFINDER_ENABLED
     // reject ABOVE_TERRAIN frame if the rangefinder is missing or unhealthy
-    if (frame == Location::AltFrame::ABOVE_TERRAIN && !sub.rangefinder_alt_ok()) {
+    if (alt_frame == Location::AltFrame::ABOVE_TERRAIN && !sub.rangefinder_alt_ok()) {
         gcs().send_text(MAV_SEVERITY_WARNING, "Terrain data (rangefinder) not available");
         return false;
     }
@@ -324,10 +324,10 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
     update_time_ms = AP_HAL::millis();
     posvel_pos_target_cm = destination.topostype();
     posvel_vel_target_cms = velocity;
-    posvel_frame = frame;
+    posvel_frame = alt_frame;
 
 #if AP_RANGEFINDER_ENABLED
-    if (frame == Location::AltFrame::ABOVE_TERRAIN) {
+    if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
         // target offset is seafloor position estimate
         position_control->set_pos_offset_target_z_cm(sub.rangefinder_state.rangefinder_terrain_offset_cm);
 
@@ -337,8 +337,8 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
 #endif
 
 #if HAL_LOGGING_ENABLED
-    // log target TODO write frame to log
-    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
+    // log target
+    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity, alt_frame);
 #endif
 
     return true;