Rover: text message severity uniformization

Global revision of message severity values.

Required also change to the low priority function gcs_send_text_fmt()
on GCS_Mavlink.cpp to disable the automatic setting of priority on
messages sent by this function

APMrover2/GCS_Mavlink.cpp

@@ -878,7 +878,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             uint8_t result = MAV_RESULT_UNSUPPORTED;
 
             // do command
-            send_text(MAV_SEVERITY_WARNING,"command received: ");
+            send_text(MAV_SEVERITY_INFO,"command received: ");
 
             switch(packet.command) {
 
@@ -1137,10 +1137,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
         {
             // mark the firmware version in the tlog
-            send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
+            send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);
 
 #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
-            send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
+            send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
 #endif
             handle_param_request_list(msg);
             break;
@@ -1363,7 +1363,7 @@ void Rover::mavlink_delay_cb()
     }
     if (tnow - last_5s > 5000) {
         last_5s = tnow;
-        gcs_send_text(MAV_SEVERITY_WARNING, "Initialising APM...");
+        gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM...");
     }
     check_usb_mux();
 
@@ -1440,10 +1440,10 @@ void Rover::gcs_send_text(MAV_SEVERITY severity, const char *str)
  *  only one fits in the queue, so if you send more than one before the
  *  last one gets into the serial buffer then the old one will be lost
  */
-void Rover::gcs_send_text_fmt(const char *fmt, ...)
+void Rover::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
 {
     va_list arg_list;
-    gcs[0].pending_status.severity = (uint8_t)MAV_SEVERITY_WARNING;
+    gcs[0].pending_status.severity = (uint8_t)severity;
     va_start(arg_list, fmt);
     hal.util->vsnprintf((char *)gcs[0].pending_status.text,
             sizeof(gcs[0].pending_status.text), fmt, arg_list);

APMrover2/Log.cpp

@@ -391,9 +391,9 @@ void Rover::log_init(void)
         gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted");
         g.log_bitmask.set(0);
     } else if (DataFlash.NeedPrep()) {
-        gcs_send_text(MAV_SEVERITY_WARNING, "Preparing log system");
+        gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
         DataFlash.Prep();
-        gcs_send_text(MAV_SEVERITY_WARNING, "Prepared log system");
+        gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
         for (uint8_t i=0; i<num_gcs; i++) {
             gcs[i].reset_cli_timeout();
         }

APMrover2/Rover.h

@@ -489,7 +489,7 @@ private:
     bool should_log(uint32_t mask);
     void frsky_telemetry_send(void);
     void print_hit_enter();    
-    void gcs_send_text_fmt(const char *fmt, ...);
+    void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
     void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
     bool start_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);

APMrover2/Steering.cpp

@@ -57,7 +57,7 @@ bool Rover::auto_check_trigger(void)
     if (!is_zero(g.auto_kickstart)) {
         float xaccel = ins.get_accel().x;
         if (xaccel >= g.auto_kickstart) {
-            gcs_send_text_fmt("Triggered AUTO xaccel=%.1f", (double)xaccel);
+            gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", (double)xaccel);
             auto_triggered = true;
             return true;            
         }

APMrover2/commands.cpp

@@ -30,7 +30,7 @@ void Rover::set_next_WP(const struct Location& loc)
     // location as the previous waypoint, to prevent immediately
     // considering the waypoint complete
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
-        gcs_send_text(MAV_SEVERITY_WARNING, "Resetting prev_WP");
+        gcs_send_text(MAV_SEVERITY_NOTICE, "Resetting prev_WP");
         prev_WP = current_loc;
     }
 
@@ -63,7 +63,7 @@ void Rover::init_home()
         return;
     }
 
-	gcs_send_text(MAV_SEVERITY_WARNING, "init home");
+	gcs_send_text(MAV_SEVERITY_INFO, "init home");
 
     ahrs.set_home(gps.location());
 	home_is_set = true;

APMrover2/commands_logic.cpp

@@ -17,7 +17,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
         return false;
     }
 
-    gcs_send_text_fmt("Executing command ID #%i",cmd.id);
+    gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing command ID #%i",cmd.id);
 
     // remember the course of our next navigation leg
     next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
@@ -117,7 +117,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
 void Rover::exit_mission()
 {
     if (control_mode == AUTO) {
-        gcs_send_text_fmt("No commands. Can't set AUTO - setting HOLD");
+        gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO - setting HOLD");
         set_mode(HOLD);
     }
 }
@@ -203,7 +203,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         // Check if we need to loiter at this waypoint
         if (loiter_time_max > 0) {
             if (loiter_time == 0) {  // check if we are just starting loiter
-                gcs_send_text_fmt("Reached Waypoint #%i - Loiter for %i seconds",
+                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Waypoint #%i - Loiter for %i seconds",
                                   (unsigned)cmd.index,
                                   (unsigned)loiter_time_max);
                 // record the current time i.e. start timer
@@ -214,7 +214,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
                 return false;
             }
         } else {
-            gcs_send_text_fmt("Reached Waypoint #%i dist %um",
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Waypoint #%i dist %um",
                               (unsigned)cmd.index,
                               (unsigned)get_distance(current_loc, next_WP));
         }
@@ -228,7 +228,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         // check if we have gone futher past the wp then last time and output new message if we have
         if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) {
             distance_past_wp = get_distance(current_loc, next_WP);
-            gcs_send_text_fmt("Passed Waypoint #%i dist %um",
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed Waypoint #%i dist %um",
                               (unsigned)cmd.index,
                               (unsigned)distance_past_wp);
         }
@@ -248,14 +248,14 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 bool Rover::verify_RTL()
 {
 	if (wp_distance <= g.waypoint_radius) {
-		gcs_send_text(MAV_SEVERITY_WARNING,"Reached Destination");
+		gcs_send_text(MAV_SEVERITY_INFO,"Reached Destination");
                 rtl_complete = true;
 		return true;
 	}
 
     // have we gone past the waypoint?
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
-        gcs_send_text_fmt("Reached Destination: Distance away %um",
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Destination: Distance away %um",
                           (unsigned)get_distance(current_loc, next_WP));
         rtl_complete = true;
         return true;
@@ -309,12 +309,12 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.content.speed.target_ms > 0) {
         g.speed_cruise.set(cmd.content.speed.target_ms);
-        gcs_send_text_fmt("Cruise speed: %.1f m/s", (double)g.speed_cruise.get());
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", (double)g.speed_cruise.get());
     }
 
 	if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
 		g.throttle_cruise.set(cmd.content.speed.throttle_pct);
-        gcs_send_text_fmt("Cruise throttle: %.1f", g.throttle_cruise.get());
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", g.throttle_cruise.get());
     }
 }
 

APMrover2/navigation.cpp

@@ -21,11 +21,6 @@ void Rover::navigate()
 	// ----------------------------
 	wp_distance = get_distance(current_loc, next_WP);
 
-	if (wp_distance < 0){
-		gcs_send_text(MAV_SEVERITY_CRITICAL,"<navigate> WP error - distance < 0");
-		return;
-	}
-
 	// control mode specific updates to nav_bearing
 	// --------------------------------------------
 	update_navigation();

APMrover2/sensors.cpp

@@ -4,9 +4,9 @@
 
 void Rover::init_barometer(void)
 {
-    gcs_send_text(MAV_SEVERITY_WARNING, "Calibrating barometer");    
+    gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
     barometer.calibrate();
-    gcs_send_text(MAV_SEVERITY_WARNING, "barometer calibration complete");
+    gcs_send_text(MAV_SEVERITY_INFO, "barometer calibration complete");
 }
 
 void Rover::init_sonar(void)
@@ -56,7 +56,7 @@ void Rover::read_sonars(void)
                 obstacle.detected_count++;
             }
             if (obstacle.detected_count == g.sonar_debounce) {
-                gcs_send_text_fmt("Sonar1 obstacle %u cm",
+                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm",
                                   (unsigned)obstacle.sonar1_distance_cm);
             }
             obstacle.detected_time_ms = hal.scheduler->millis();
@@ -67,7 +67,7 @@ void Rover::read_sonars(void)
                 obstacle.detected_count++;
             }
             if (obstacle.detected_count == g.sonar_debounce) {
-                gcs_send_text_fmt("Sonar2 obstacle %u cm",
+                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm",
                                   (unsigned)obstacle.sonar2_distance_cm);
             }
             obstacle.detected_time_ms = hal.scheduler->millis();
@@ -83,7 +83,7 @@ void Rover::read_sonars(void)
                 obstacle.detected_count++;
             }
             if (obstacle.detected_count == g.sonar_debounce) {
-                gcs_send_text_fmt("Sonar obstacle %u cm",
+                gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Sonar obstacle %u cm",
                                   (unsigned)obstacle.sonar1_distance_cm);
             }
             obstacle.detected_time_ms = hal.scheduler->millis();
@@ -96,7 +96,7 @@ void Rover::read_sonars(void)
     // no object detected - reset after the turn time
     if (obstacle.detected_count >= g.sonar_debounce &&
         hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { 
-        gcs_send_text_fmt("Obstacle passed");
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed");
         obstacle.detected_count = 0;
         obstacle.turn_angle = 0;
     }

APMrover2/system.cpp

@@ -213,11 +213,11 @@ void Rover::init_ardupilot()
 void Rover::startup_ground(void)
 {
     set_mode(INITIALISING);
-
-	gcs_send_text(MAV_SEVERITY_WARNING,"<startup_ground> GROUND START");
+    
+	gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> GROUND START");
 
 	#if(GROUND_START_DELAY > 0)
-		gcs_send_text(MAV_SEVERITY_WARNING,"<startup_ground> With Delay");
+		gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With Delay");
 		delay(GROUND_START_DELAY * 1000);
 	#endif
 
@@ -241,7 +241,7 @@ void Rover::startup_ground(void)
     ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
     ins.set_dataflash(&DataFlash);
 
-	gcs_send_text(MAV_SEVERITY_WARNING,"\n\n Ready to drive.");
+	gcs_send_text(MAV_SEVERITY_INFO,"\n\n Ready to drive.");
 }
 
 /*
@@ -352,7 +352,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
     }
     if (failsafe.triggered != 0 && failsafe.bits == 0) {
         // a failsafe event has ended
-        gcs_send_text_fmt("Failsafe ended");
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Failsafe ended");
     }
 
     failsafe.triggered &= failsafe.bits;
@@ -363,7 +363,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
         control_mode != RTL &&
         control_mode != HOLD) {
         failsafe.triggered = failsafe.bits;
-        gcs_send_text_fmt("Failsafe trigger 0x%x", (unsigned)failsafe.triggered);
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", (unsigned)failsafe.triggered);
         switch (g.fs_action) {
         case 0:
             break;
@@ -379,12 +379,12 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
 
 void Rover::startup_INS_ground(void)
 {
-    gcs_send_text(MAV_SEVERITY_ALERT, "Warming up ADC...");
+    gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC...");
  	mavlink_delay(500);
 
 	// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
 	// -----------------------
-    gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration; do not move vehicle");
+    gcs_send_text(MAV_SEVERITY_INFO, "Beginning INS calibration; do not move vehicle");
 	mavlink_delay(1000);
 
     ahrs.init();