ArduPilot · tridge · Oct 1, 2024 · Sep 27, 2024 · Sep 27, 2024 · Sep 27, 2024
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
@@ -671,13 +671,6 @@ bool ModeAuto::set_speed_down(float speed_down_cms)
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
 bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 {
-#if HAL_LOGGING_ENABLED
-    // To-Do: logging when new commands start/end
-    if (copter.should_log(MASK_LOG_CMD)) {
-        copter.logger.Write_Mission_Cmd(mission, cmd);
-    }
-#endif
-
     switch(cmd.id) {
 
     ///

diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp
@@ -160,6 +160,9 @@ void Copter::init_ardupilot()
 #if MODE_AUTO_ENABLED
     // initialise mission library
     mode_auto.mission.init();
+#if HAL_LOGGING_ENABLED
+    mode_auto.mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
+#endif
 #endif
 
 #if MODE_SMARTRTL_ENABLED

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -12,13 +12,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     plane.target_altitude.terrain_following_pending = false;
 #endif
 
-#if HAL_LOGGING_ENABLED
-    // log when new commands start
-    if (should_log(MASK_LOG_CMD)) {
-        logger.Write_Mission_Cmd(mission, cmd);
-    }
-#endif
-
     // special handling for nav vs non-nav commands
     if (AP_Mission::is_nav_cmd(cmd)) {
         // set takeoff_complete to true so we don't add extra elevator

diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
@@ -127,6 +127,9 @@ void Plane::init_ardupilot()
 
     // initialise mission library
     mission.init();
+#if HAL_LOGGING_ENABLED
+    mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
+#endif
 
     // initialise AP_Logger library
 #if HAL_LOGGING_ENABLED

diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp
@@ -7,13 +7,6 @@ static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCAT
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
 bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
 {
-#if HAL_LOGGING_ENABLED
-    // To-Do: logging when new commands start/end
-    if (should_log(MASK_LOG_CMD)) {
-        logger.Write_Mission_Cmd(mission, cmd);
-    }
-#endif
-
     const Location &target_loc = cmd.content.location;
     auto alt_frame = target_loc.get_alt_frame();
 

diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
@@ -140,6 +140,9 @@ void Sub::init_ardupilot()
 
     // initialise mission library
     mission.init();
+#if HAL_LOGGING_ENABLED
+    mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
+#endif
 
     // initialise AP_Logger library
 #if HAL_LOGGING_ENABLED

diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp
@@ -512,13 +512,6 @@ void ModeAuto::send_guided_position_target()
 /********************************************************************************/
 bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 {
-#if HAL_LOGGING_ENABLED
-    // log when new commands start
-    if (rover.should_log(MASK_LOG_CMD)) {
-        rover.logger.Write_Mission_Cmd(mission, cmd);
-    }
-#endif
-
     switch (cmd.id) {
     case MAV_CMD_NAV_WAYPOINT:  // Navigate to Waypoint
         return do_nav_wp(cmd, false);

diff --git a/Rover/system.cpp b/Rover/system.cpp
@@ -125,6 +125,9 @@ void Rover::init_ardupilot()
 #if AP_MISSION_ENABLED
     // initialise mission library
     mode_auto.mission.init();
+#if HAL_LOGGING_ENABLED
+    mode_auto.mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
+#endif
 #endif
 
     // initialise AP_Logger library

diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp
@@ -896,9 +896,10 @@ void AP_Logger::Write_Parameter(const char *name, float value)
 }
 
 void AP_Logger::Write_Mission_Cmd(const AP_Mission &mission,
-                                            const AP_Mission::Mission_Command &cmd)
+                                  const AP_Mission::Mission_Command &cmd,
+                                  LogMessages id)
 {
-    FOR_EACH_BACKEND(Write_Mission_Cmd(mission, cmd));
+    FOR_EACH_BACKEND(Write_Mission_Cmd(mission, cmd, id));
 }
 
 #if HAL_RALLY_ENABLED

diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
@@ -273,8 +273,15 @@ class AP_Logger
                        uint8_t source_component,
                        MAV_RESULT result,
                        bool was_command_long=false);
+    void Write_MISE(const AP_Mission &mission, const AP_Mission::Mission_Command &cmd) {
+        Write_Mission_Cmd(mission, cmd, LOG_MISE_MSG);
+    }
+    void Write_CMD(const AP_Mission &mission, const AP_Mission::Mission_Command &cmd) {
+        Write_Mission_Cmd(mission, cmd, LOG_CMD_MSG);
+    }
     void Write_Mission_Cmd(const AP_Mission &mission,
-                               const AP_Mission::Mission_Command &cmd);
+                           const AP_Mission::Mission_Command &cmd,
+                           LogMessages id);
     void Write_RallyPoint(uint8_t total,
                           uint8_t sequence,
                           const class RallyLocation &rally_point);

diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h
@@ -140,7 +140,8 @@ class AP_Logger_Backend
     bool Write_Message(const char *message);
     bool Write_MessageF(const char *fmt, ...);
     bool Write_Mission_Cmd(const AP_Mission &mission,
-                               const AP_Mission::Mission_Command &cmd);
+                           const AP_Mission::Mission_Command &cmd,
+                           LogMessages id);
     bool Write_Mode(uint8_t mode, const ModeReason reason);
     bool Write_Parameter(const char *name, float value, float default_val);
     bool Write_Parameter(const AP_Param *ap,

diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp
@@ -300,12 +300,13 @@ void AP_Logger::Write_Command(const mavlink_command_int_t &packet,
 }
 
 bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
-                                              const AP_Mission::Mission_Command &cmd)
+                                          const AP_Mission::Mission_Command &cmd,
+                                          LogMessages msgid)
 {
     mavlink_mission_item_int_t mav_cmd = {};
     AP_Mission::mission_cmd_to_mavlink_int(cmd,mav_cmd);
-    const struct log_Cmd pkt{
-        LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
+    const struct log_CMD pkt{
+        LOG_PACKET_HEADER_INIT(msgid),
         time_us         : AP_HAL::micros64(),
         command_total   : mission.num_commands(),
         sequence        : mav_cmd.seq,

diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
@@ -146,6 +146,7 @@ const struct MultiplierStructure log_Multipliers[] = {
 #include <AP_Landing/LogStructure.h>
 #include <AC_AttitudeControl/LogStructure.h>
 #include <AP_HAL/LogStructure.h>
+#include <AP_Mission/LogStructure.h>
 
 // structure used to define logging format
 // It is packed on ChibiOS to save flash space; however, this causes problems
@@ -350,22 +351,6 @@ struct PACKED log_MCU {
     float MCU_voltage_max;
 };
 
-struct PACKED log_Cmd {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    uint16_t command_total;
-    uint16_t sequence;
-    uint16_t command;
-    float param1;
-    float param2;
-    float param3;
-    float param4;
-    int32_t latitude;
-    int32_t longitude;
-    float altitude;
-    uint8_t frame;
-};
-
 struct PACKED log_MAVLink_Command {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -712,21 +697,6 @@ struct PACKED log_VER {
 // @Field: TR: innovation test ratio
 // @Field: Pri: True if sensor is the primary sensor
 
-// @LoggerMessage: CMD
-// @Description: Executed mission command information
-// @Field: TimeUS: Time since system startup
-// @Field: CTot: Total number of mission commands
-// @Field: CNum: This command's offset in mission
-// @Field: CId: Command type
-// @Field: Prm1: Parameter 1
-// @Field: Prm2: Parameter 2
-// @Field: Prm3: Parameter 3
-// @Field: Prm4: Parameter 4
-// @Field: Lat: Command latitude
-// @Field: Lng: Command longitude
-// @Field: Alt: Command altitude
-// @Field: Frame: Frame used for position
-
 // @LoggerMessage: CSRV
 // @Description: Servo feedback data
 // @Field: TimeUS: Time since system startup
@@ -1218,8 +1188,7 @@ LOG_STRUCTURE_FROM_PRECLAND \
       "POWR","QffHHB","TimeUS,Vcc,VServo,Flags,AccFlags,Safety", "svv---", "F00---", true }, \
     { LOG_MCU_MSG, sizeof(log_MCU), \
       "MCU","Qffff","TimeUS,MTemp,MVolt,MVmin,MVmax", "sOvvv", "F0000", true }, \
-    { LOG_CMD_MSG, sizeof(log_Cmd), \
-      "CMD", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" }, \
+LOG_STRUCTURE_FROM_MISSION \
     { LOG_MAVLINK_COMMAND_MSG, sizeof(log_MAVLink_Command), \
       "MAVC", "QBBBBBHffffiifBB","TimeUS,TS,TC,SS,SC,Fr,Cmd,P1,P2,P3,P4,X,Y,Z,Res,WL", "s---------------", "F---------------" }, \
     { LOG_RADIO_MSG, sizeof(log_Radio), \
@@ -1326,7 +1295,6 @@ enum LogMessages : uint8_t {
     LOG_MCU_MSG,
     LOG_IDS_FROM_AHRS,
     LOG_SIMSTATE_MSG,
-    LOG_CMD_MSG,
     LOG_MAVLINK_COMMAND_MSG,
     LOG_RADIO_MSG,
     LOG_ATRP_MSG,
@@ -1337,6 +1305,7 @@ enum LogMessages : uint8_t {
     LOG_IDS_FROM_ESC_TELEM,
     LOG_IDS_FROM_BATTMONITOR,
     LOG_IDS_FROM_HAL_CHIBIOS,
+    LOG_IDS_FROM_MISSION,
 
     LOG_IDS_FROM_GPS,
 

diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp
@@ -437,7 +437,7 @@ void LoggerMessageWriter_WriteEntireMission::process() {
             // upon failure to write the mission we will re-read from
             // storage; this could be improved.
             if (_mission->read_cmd_from_storage(_mission_number_to_send,cmd)) {
-                if (!_logger_backend->Write_Mission_Cmd(*_mission, cmd)) {
+                if (!_logger_backend->Write_Mission_Cmd(*_mission, cmd, LOG_CMD_MSG)) {
                     return; // call me again
                 }
             }

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
@@ -17,6 +17,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <RC_Channel/RC_Channel_config.h>
 #include <AC_Fence/AC_Fence.h>
+#include <AP_Logger/AP_Logger.h>
 
 const AP_Param::GroupInfo AP_Mission::var_info[] = {
 
@@ -400,6 +401,15 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
 
 bool AP_Mission::start_command(const Mission_Command& cmd)
 {
+#if HAL_LOGGING_ENABLED
+    if (log_start_mission_item_bit != (uint32_t)-1) {
+        auto &logger = AP::logger();
+        if (logger.should_log(log_start_mission_item_bit)) {
+            logger.Write_MISE(*this, cmd);
+        }
+    }
+#endif
+
     // check for landing related commands and set flags
     if (is_landing_type_cmd(cmd.id) || cmd.id == MAV_CMD_DO_LAND_START) {
         _flags.in_landing_sequence = true;

diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
@@ -778,6 +778,10 @@ class AP_Mission
     }
 #endif
 
+#if HAL_LOGGING_ENABLED
+    void set_log_start_mission_item_bit(uint32_t bit) { log_start_mission_item_bit = bit; }
+#endif
+
 private:
     static AP_Mission *_singleton;
 
@@ -949,6 +953,11 @@ class AP_Mission
       format to take advantage of new packing
      */
     void format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const;
+
+#if HAL_LOGGING_ENABLED
+    // if not -1, this bit in LOG_BITMASK specifies whether to log a message each time we start a command:
+    uint32_t log_start_mission_item_bit = -1;
+#endif
 };
 
 namespace AP

diff --git a/libraries/AP_Mission/LogStructure.h b/libraries/AP_Mission/LogStructure.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_MISSION \
+    LOG_MISE_MSG,            \
+    LOG_CMD_MSG
+
+// @LoggerMessage: CMD
+// @Description: Uploaded mission command information
+// @Field: TimeUS: Time since system startup
+// @Field: CTot: Total number of mission commands
+// @Field: CNum: This command's offset in mission
+// @Field: CId: Command type
+// @Field: Prm1: Parameter 1
+// @Field: Prm2: Parameter 2
+// @Field: Prm3: Parameter 3
+// @Field: Prm4: Parameter 4
+// @Field: Lat: Command latitude
+// @Field: Lng: Command longitude
+// @Field: Alt: Command altitude
+// @Field: Frame: Frame used for position
+struct PACKED log_CMD {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint16_t command_total;
+    uint16_t sequence;
+    uint16_t command;
+    float param1;
+    float param2;
+    float param3;
+    float param4;
+    int32_t latitude;
+    int32_t longitude;
+    float altitude;
+    uint8_t frame;
+};
+
+// @LoggerMessage: MISE
+// @Description: Executed mission command information; emitted when we start to run an item
+// @Field: TimeUS: Time since system startup
+// @Field: CTot: Total number of mission commands
+// @Field: CNum: This command's offset in mission
+// @Field: CId: Command type
+// @Field: Prm1: Parameter 1
+// @Field: Prm2: Parameter 2
+// @Field: Prm3: Parameter 3
+// @Field: Prm4: Parameter 4
+// @Field: Lat: Command latitude
+// @Field: Lng: Command longitude
+// @Field: Alt: Command altitude
+// @Field: Frame: Frame used for position
+
+// note we currently reuse the same structure for CMD and MISE.
+#define LOG_STRUCTURE_FROM_MISSION      \
+        { LOG_CMD_MSG, sizeof(log_CMD), \
+      "CMD", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" }, \
+        { LOG_MISE_MSG, sizeof(log_CMD), \
+      "MISE", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" },