Rover: send flight mode string to Notify

APMrover2/Rover.h

@@ -522,6 +522,7 @@ private:
     void print_hit_enter();
     void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
     void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
+    void notify_mode(enum mode new_mode);
     bool start_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);

APMrover2/system.cpp

@@ -325,6 +325,8 @@ void Rover::set_mode(enum mode mode)
     if (should_log(MASK_LOG_MODE)) {
         DataFlash.Log_Write_Mode(control_mode);
     }
+
+    notify_mode(control_mode);
 }
 
 /*
@@ -459,6 +461,42 @@ void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
     }
 }
 
+// update notify with mode change
+void Rover::notify_mode(enum mode new_mode)
+{
+    notify.flags.flight_mode = new_mode;
+
+    switch (new_mode) {
+    case MANUAL:
+        notify.set_flight_mode_str("MANU");
+        break;
+    case LEARNING:
+        notify.set_flight_mode_str("LERN");
+        break;
+    case STEERING:
+        notify.set_flight_mode_str("STER");
+        break;
+    case HOLD:
+        notify.set_flight_mode_str("HOLD");
+        break;
+    case AUTO:
+        notify.set_flight_mode_str("AUTO");
+        break;
+    case RTL:
+        notify.set_flight_mode_str("RTL");
+        break;
+    case GUIDED:
+        notify.set_flight_mode_str("GUID");
+        break;
+    case INITIALISING:
+        notify.set_flight_mode_str("INIT");
+        break;
+    default:
+        notify.set_flight_mode_str("----");
+        break;
+    }
+}
+
 /*
   check a digitial pin for high,low (1/0)
  */