Copter, AP_Mission: Send progress for individual mission items (take 2)

ArduCopter/GCS_Mavlink.cpp

@@ -512,6 +512,9 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 #endif
     MSG_MEMINFO,
     MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    MSG_NAV_CONTROLLER_PROGRESS,
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
     MSG_GPS_RAW,
     MSG_GPS_RTK,
 #if GPS_MAX_RECEIVERS > 1

ArduCopter/mode.h

@@ -671,7 +671,7 @@ class ModeAuto : public Mode {
     bool verify_payload_place();
     bool verify_loiter_unlimited();
     bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
-    bool verify_loiter_to_alt() const;
+    bool verify_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     bool verify_RTL();
     bool verify_wait_delay();
     bool verify_within_distance();

ArduCopter/mode_auto.cpp

@@ -949,7 +949,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_LOITER_TO_ALT:
-        return verify_loiter_to_alt();
+        return verify_loiter_to_alt(cmd);
 
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
         cmd_complete = verify_RTL();
@@ -2123,6 +2123,10 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 {
     // return immediately if we haven't reached our destination
     if (!copter.wp_nav->reached_wp_destination()) {
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+        const float distance_m = copter.wp_nav->get_wp_distance_to_destination() / 100.0f;
+        mission.set_item_progress_distance_remaining(cmd.index, distance_m);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
         return false;
     }
 
@@ -2131,8 +2135,13 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
         loiter_time = millis();
     }
 
+    const uint32_t loiter_time_elapsed_s = (millis() - loiter_time) / 1000;
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    mission.set_item_progress_time_elapsed(cmd.index, loiter_time_elapsed_s, loiter_time_max);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+
     // check if loiter timer has run out
-    if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
+    if (loiter_time_elapsed_s >= loiter_time_max) {
         gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
         return true;
     }
@@ -2142,12 +2151,21 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 
 // verify_loiter_to_alt - check if we have reached both destination
 // (roughly) and altitude (precisely)
-bool ModeAuto::verify_loiter_to_alt() const
+bool ModeAuto::verify_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
 {
     if (loiter_to_alt.reached_destination_xy &&
         loiter_to_alt.reached_alt) {
         return true;
     }
+
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    // Copter nav flies a straight line in 3D space to get to the Loiter Alt
+    // waypoint (it doesn't fly to the xy pos then ascend/descend).
+    const Vector2f vector_cm = Vector2f(copter.wp_nav->get_wp_distance_to_destination(), (copter.current_loc.alt - loiter_to_alt.alt));
+    const float distance_m = vector_cm.length() / 100.0f;
+    mission.set_item_progress_distance_remaining(cmd.index, distance_m);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+
     return false;
 }
 
@@ -2198,6 +2216,10 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     // check if we have reached the waypoint
     if ( !copter.wp_nav->reached_wp_destination() ) {
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+        float distance_m = copter.wp_nav->get_wp_distance_to_destination() / 100.0f;
+        mission.set_item_progress_distance_remaining(cmd.index, distance_m);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
         return false;
     }
 
@@ -2210,8 +2232,17 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         }
     }
 
+    const uint32_t loiter_time_elapsed_s = (millis() - loiter_time) / 1000;
+
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    // only publish progress if we have a loiter time defined
+    if (loiter_time_max > 0) {
+        mission.set_item_progress_time_elapsed(cmd.index, loiter_time_elapsed_s, loiter_time_max);
+    }
+#endif //AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+
     // check if timer has run out
-    if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
+    if (loiter_time_elapsed_s >= loiter_time_max) {
         if (loiter_time_max == 0) {
             // play a tone
             AP_Notify::events.waypoint_complete = 1;
@@ -2231,20 +2262,32 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
             // start circling
             circle_start();
         }
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+        float distance_m = copter.wp_nav->get_wp_distance_to_destination() / 100.0f;
+        mission.set_item_progress_distance_remaining(cmd.index, distance_m);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
         return false;
     }
 
     const float turns = cmd.get_loiter_turns();
+    const float turns_completed = fabsf(copter.circle_nav->get_angle_total()/float(M_2PI));
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    mission.set_item_progress_count_completed(cmd.index, turns_completed, turns);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
 
     // check if we have completed circling
-    return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= turns;
+    return turns_completed >= turns;
 }
 
 // verify_spline_wp - check if we have reached the next way point using spline
 bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
     // check if we have reached the waypoint
     if ( !copter.wp_nav->reached_wp_destination() ) {
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+        float distance_m = copter.wp_nav->get_wp_distance_to_destination() / 100.0f;
+        mission.set_item_progress_distance_remaining(cmd.index, distance_m);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
         return false;
     }
 
@@ -2278,7 +2321,16 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 // verify_nav_delay - check if we have waited long enough
 bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
 {
-    if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) {
+    const uint32_t delay_time_elapsed_ms = millis() - nav_delay_time_start_ms;
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    mission.set_item_progress_time_elapsed(
+        cmd.index,
+        delay_time_elapsed_ms / 1000.0f,
+        nav_delay_time_max_ms / 1000.0f
+    );
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+
+    if (delay_time_elapsed_ms > nav_delay_time_max_ms) {
         nav_delay_time_max_ms = 0;
         return true;
     }

Tools/scripts/build_options.py

@@ -356,6 +356,7 @@ def __init__(self,
     Feature('MAVLink', 'MAVLINK_MSG_RC_CHANNELS_RAW', 'AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED', 'Enable sending of RC_CHANNELS_RAW mavlink messages', 0, None),  # noqa
     Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP Protocol', 0, None),  # noqa
     Feature('MAVLink', 'MAV_CMD_SET_HAGL', 'AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED', 'Enable MAVLink HAGL command', 0, None),  # noqa
+    Feature('MAVLink', 'MAV_MSG_NAV_CONTROLLER_PROGRESS', 'AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED', 'Enable sending MAVLink NAV_CONTROLLER_PROGRESS message', 0, None),  # noqa
 
     Feature('Developer', 'KILL_IMU', 'AP_INERTIALSENSOR_KILL_IMU_ENABLED', 'Allow IMUs to be disabled at runtime', 0, None),
     Feature('Developer', 'CRASHCATCHER', 'AP_CRASHDUMP_ENABLED', 'Enable CrashCatcher', 0, None),

Tools/scripts/extract_features.py

@@ -244,6 +244,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED', 'GCS_MAVLINK::send_rc_channels_raw\b'),
             ('AP_MAVLINK_FTP_ENABLED', 'GCS_MAVLINK::ftp_worker'),
             ('AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED', 'Plane::get_external_HAGL'),
+            ('AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED', 'GCS_MAVLINK::send_nav_controller_progress'),
 
             ('AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'AP_DroneCAN::SRV_send_himark'),
             ('AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'AP_DroneCAN::hobbywing_ESC_update'),

libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc

@@ -133,3 +133,6 @@ define AP_AIRSPEED_DRONECAN_ENABLED HAL_ENABLE_DRONECAN_DRIVERS
 define AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED 0
 # don't need the payload place flight behaviour either:
 define AC_PAYLOAD_PLACE_ENABLED 0
+
+# disable nav controller progress message
+define AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED 0

libraries/AP_Mission/AP_Mission.cpp

@@ -100,6 +100,10 @@ void AP_Mission::init()
         clear();
     }
 
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    clear_item_progress();
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+
     _last_change_time_ms = AP_HAL::millis();
 }
 
@@ -122,6 +126,9 @@ void AP_Mission::start()
 void AP_Mission::stop()
 {
     _flags.state = MISSION_STOPPED;
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    clear_item_progress();
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
 }
 
 /// resume - continues the mission execution from where we last left off
@@ -270,6 +277,9 @@ void AP_Mission::reset()
     _prev_nav_cmd_id       = AP_MISSION_CMD_ID_NONE;
     init_jump_tracking();
     reset_wp_history();
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    clear_item_progress();
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
 }
 
 /// clear - clears out mission
@@ -3084,6 +3094,89 @@ bool AP_Mission::jump_to_abort_landing_sequence(void)
 }
 #endif // AP_SCRIPTING_ENABLED
 
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+void AP_Mission::set_item_progress(uint16_t index, float distance_remaining_m, float time_remaining_s, float count_remaining, uint8_t percentage_complete)
+{
+    _current_item_progress.index = index;
+    _current_item_progress.distance_remaining_m = distance_remaining_m;
+    _current_item_progress.time_remaining_s = time_remaining_s;
+    _current_item_progress.count_remaining = count_remaining;
+    _current_item_progress.percentage_complete = percentage_complete;
+}
+
+AP_Mission::ItemProgress AP_Mission::get_item_progress(uint16_t index) const
+{
+    if (index && (index == _current_item_progress.index)) {
+        return _current_item_progress;
+    }
+    return kInvalidProgress;
+}
+
+void AP_Mission::clear_item_progress(void)
+{
+    _current_item_progress = kInvalidProgress;
+}
+
+uint8_t AP_Mission::calc_item_percentage_complete(float completed, float total) const
+{
+    uint8_t percentage_complete;
+    if (isnan(total) || isnan(completed)) {
+        percentage_complete = kInvalidProgress.percentage_complete;
+    } else if  ((total <= 0.0f) || (completed < 0.0f)) {
+        percentage_complete = kInvalidProgress.percentage_complete;
+    } else if (completed >= total) {
+        percentage_complete = 100;
+    } else {
+        percentage_complete = (completed * 100.0f) / total;
+    }
+    return percentage_complete;
+}
+
+void AP_Mission::set_item_progress_distance_remaining(uint16_t index, float remaining_m, float total_m)
+{
+    const uint8_t percentage_complete = calc_item_percentage_complete(total_m - remaining_m, total_m);
+    set_item_progress(index, remaining_m, kInvalidProgress.time_remaining_s, kInvalidProgress.count_remaining, percentage_complete);
+}
+
+void AP_Mission::set_item_progress_time_remaining(uint16_t index, float remaining_s, float total_s)
+{
+    const uint8_t percentage_complete = calc_item_percentage_complete(total_s - remaining_s, total_s);
+    set_item_progress(index, kInvalidProgress.distance_remaining_m, remaining_s, kInvalidProgress.count_remaining, percentage_complete);
+}
+
+void AP_Mission::set_item_progress_count_remaining(uint16_t index, float remaining, float total)
+{
+    const uint8_t percentage_complete = calc_item_percentage_complete(total - remaining, total);
+    set_item_progress(index, kInvalidProgress.distance_remaining_m, kInvalidProgress.time_remaining_s, remaining, percentage_complete);
+}
+
+void AP_Mission::set_item_progress_distance_traveled(uint16_t index, float traveled_m, float total_m)
+{
+    if (total_m > traveled_m) {
+        set_item_progress_distance_remaining(index, total_m - traveled_m, total_m);
+    } else {
+        set_item_progress(index, 0.0f, kInvalidProgress.time_remaining_s, kInvalidProgress.count_remaining, 100);
+    }
+}
+
+void AP_Mission::set_item_progress_time_elapsed(uint16_t index, float elapsed_s, float total_s)
+{
+    if (total_s > elapsed_s) {
+        set_item_progress_time_remaining(index, total_s - elapsed_s, total_s);
+    } else {
+        set_item_progress(index, kInvalidProgress.distance_remaining_m, 0.0f, kInvalidProgress.count_remaining, 100);
+    }
+}
+
+void AP_Mission::set_item_progress_count_completed(uint16_t index, float completed, float total)
+{
+    if (total > completed) {
+        set_item_progress_count_remaining(index, total - completed, total);
+    } else {
+        set_item_progress(index, kInvalidProgress.distance_remaining_m, kInvalidProgress.time_remaining_s, 0.0f, 100);
+    }
+}
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
 
 // singleton instance
 AP_Mission *AP_Mission::_singleton;

libraries/AP_Mission/AP_Mission.h

@@ -437,6 +437,39 @@ class AP_Mission
         }
     };
 
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    struct ItemProgress {
+        float distance_remaining_m;
+        float time_remaining_s;
+        float count_remaining;
+        uint16_t index;
+        uint8_t percentage_complete;
+    };
+
+    void set_item_progress(uint16_t index, float distance_remaining_m, float time_remaining_s, float count_remaining, uint8_t percentage_complete);
+    ItemProgress get_item_progress(uint16_t index) const;
+    void clear_item_progress(void);
+
+    // Populate the 'remaining' fields of the NAV_CONTROLLER_PROGRESS message with distance remaining.
+    // Note: If total is set to 0, the reported percentage complete is set to 255 (invalid).
+    void set_item_progress_distance_remaining(uint16_t index, float remaining_m, float total_m=0);
+    // Populate the 'remaining' fields of the NAV_CONTROLLER_PROGRESS message with time remaining.
+    // Note: If total is set to 0, the reported percentage complete is set to 255 (invalid).
+    void set_item_progress_time_remaining(uint16_t index, float remaining_s, float total_s=0);
+    // Populate the 'remaining' fields of the NAV_CONTROLLER_PROGRESS message with count remaining.
+    // Note: If total is set to 0, the reported percentage complete is set to 255 (invalid).
+    void set_item_progress_count_remaining(uint16_t index, float remaining, float total=0);
+
+    // Populate the 'remaining' fields of the NAV_CONTROLLER_PROGRESS message with distance traveled.
+    // Note: This gets converted to a distance remaining, so requires a total greater than 0.
+    void set_item_progress_distance_traveled(uint16_t index, float traveled_m, float total_m);
+    // Populate the 'remaining' fields of the NAV_CONTROLLER_PROGRESS message with time elapsed.
+    // Note: This gets converted to a time remaining, so requires a total greater than 0.
+    void set_item_progress_time_elapsed(uint16_t index, float elapsed_s, float total_s);
+    // Populate the 'remaining' fields of the NAV_CONTROLLER_PROGRESS message with count completed.
+    // Note: This gets converted to a count remaining, so requires a total greater than 0.
+    void set_item_progress_count_completed(uint16_t index, float completed, float total);
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
 
     // main program function pointers
     FUNCTOR_TYPEDEF(mission_cmd_fn_t, bool, const Mission_Command&);
@@ -949,6 +982,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 AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    ItemProgress _current_item_progress;
+    const ItemProgress kInvalidProgress { NAN, NAN, NAN, UINT16_MAX, UINT8_MAX, };
+    uint8_t calc_item_percentage_complete(float completed, float total) const;
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
 };
 
 namespace AP

libraries/GCS_MAVLink/GCS.h

@@ -402,6 +402,10 @@ class GCS_MAVLINK
     void send_uavionix_adsb_out_status() const;
     void send_autopilot_state_for_gimbal_device() const;
 
+#if AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+    void send_nav_controller_progress() const;
+#endif // AP_MAVLINK_MAV_MSG_NAV_CONTROLLER_PROGRESS_ENABLED
+
     // lock a channel, preventing use by MAVLink
     void lock(bool _lock) {
         _locked = _lock;