Sub: use send_text method on the GCS singleton

ArduSub/AP_Arming_Sub.cpp

@@ -37,7 +37,7 @@ bool AP_Arming_Sub::rc_check(bool report)
     }
 
     if (report && !ret) {
-        sub.gcs_send_text(MAV_SEVERITY_CRITICAL, message_fail);
+        gcs().send_text(MAV_SEVERITY_CRITICAL, message_fail);
     }
 
     return ret;

ArduSub/ArduSub.cpp

@@ -107,7 +107,7 @@ void Sub::perf_update(void)
         Log_Write_Performance();
     }
     if (scheduler.debug()) {
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu",
+        gcs().send_text(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu",
                           (unsigned)perf_info_get_num_long_running(),
                           (unsigned)perf_info_get_num_loops(),
                           (unsigned long)perf_info_get_max_time(),

ArduSub/GCS_Mavlink.cpp

@@ -1075,7 +1075,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_PREFLIGHT_STORAGE:
             if (is_equal(packet.param1, 2.0f)) {
                 AP_Param::erase_all();
-                sub.gcs_send_text(MAV_SEVERITY_WARNING, "All parameters reset, reboot board");
+                gcs().send_text(MAV_SEVERITY_WARNING, "All parameters reset, reboot board");
                 result= MAV_RESULT_ACCEPTED;
             }
             break;
@@ -1264,7 +1264,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             } else if (is_equal(packet.param6,1.0f)) {
                 // compassmot calibration
                 //result = sub.mavlink_compassmot(chan);
-                sub.gcs_send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported");
+                gcs().send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported");
                 result = MAV_RESULT_UNSUPPORTED;
             }
             break;
@@ -1826,7 +1826,7 @@ void Sub::mavlink_delay_cb()
     }
     if (tnow - last_5s > 5000) {
         last_5s = tnow;
-        gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM");
+        gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
     }
 
     DataFlash.EnableWrites(true);
@@ -1864,23 +1864,3 @@ void Sub::gcs_check_input(void)
 {
     gcs().update();
 }
-
-void Sub::gcs_send_text(MAV_SEVERITY severity, const char *str)
-{
-    gcs().send_statustext(severity, 0xFF, str);
-}
-
-/*
- *  send a low priority formatted message to the GCS
- *  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 Sub::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
-{
-    char str[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
-    va_list arg_list;
-    va_start(arg_list, fmt);
-    va_end(arg_list);
-    hal.util->vsnprintf((char *)str, sizeof(str), fmt, arg_list);
-    gcs().send_statustext(severity, 0xFF, str);
-}

ArduSub/Log.cpp

@@ -8,9 +8,9 @@
 
 void Sub::do_erase_logs(void)
 {
-    gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs");
+    gcs().send_text(MAV_SEVERITY_INFO, "Erasing logs");
     DataFlash.EraseAll();
-    gcs_send_text(MAV_SEVERITY_INFO, "Log erase complete");
+    gcs().send_text(MAV_SEVERITY_INFO, "Log erase complete");
 }
 
 // Write a Current data packet

ArduSub/Sub.h

@@ -497,7 +497,6 @@ private:
     void gcs_send_mission_item_reached_message(uint16_t mission_index);
     void gcs_data_stream_send(void);
     void gcs_check_input(void);
-    void gcs_send_text(MAV_SEVERITY severity, const char *str);
     void do_erase_logs(void);
     void Log_Write_Current();
     void Log_Write_Optflow();
@@ -666,7 +665,6 @@ private:
     bool ekf_position_ok();
     bool optflow_position_ok();
     bool should_log(uint32_t mask);
-    void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
     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);

ArduSub/commands_logic.cpp

@@ -14,13 +14,13 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
 
     // target alt must be negative (underwater)
     if (target_loc.alt > 0.0f) {
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
+        gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
         return true;
     }
 
     // only tested/supported alt frame so far is ALT_FRAME_ABOVE_HOME, where Home alt is always water's surface ie zero depth
     if (target_loc.get_alt_frame() != Location_Class::ALT_FRAME_ABOVE_HOME) {
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "BAD NAV ALT_FRAME %d", (int8_t)target_loc.get_alt_frame());
+        gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT_FRAME %d", (int8_t)target_loc.get_alt_frame());
         return true;
     }
 
@@ -248,7 +248,7 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
 
     default:
         // error message
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
+        gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
         // return true if we do not recognize the command so that we move on to the next command
         return true;
     }
@@ -528,7 +528,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd)
         // absolute delay to utc time
         nav_delay_time_max = hal.util->get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
     }
-    gcs_send_text_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000));
+    gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000));
 }
 
 #if GRIPPER_ENABLED == ENABLED
@@ -585,7 +585,7 @@ bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 
     // check if timer has run out
     if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
-        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
+        gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
         return true;
     } else {
         return false;
@@ -700,7 +700,7 @@ bool Sub::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
 
     // check if timer has run out
     if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
-        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
+        gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
         return true;
     } else {
         return false;

ArduSub/control_althold.cpp

@@ -10,7 +10,7 @@ bool Sub::althold_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
     if (!ap.depth_sensor_present || failsafe.sensor_health) { // can't hold depth without a depth sensor, exit immediately.
-        gcs_send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
+        gcs().send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
         return false;
     }
 #endif

ArduSub/control_auto.cpp

@@ -659,7 +659,7 @@ bool Sub::auto_terrain_recover_start()
     pos_control.set_alt_target(inertial_nav.get_altitude());
     pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
-    gcs_send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
+    gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
     return true;
 }
 
@@ -704,7 +704,7 @@ void Sub::auto_terrain_recover_run()
 
             // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
             if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) {
-                gcs_send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!");
+                gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!");
                 failsafe_terrain_set_status(true); // Reset failsafe timers
                 failsafe.terrain = false; // Clear flag
                 auto_mode = Auto_Loiter; // Switch back to loiter for next iteration
@@ -719,7 +719,7 @@ void Sub::auto_terrain_recover_run()
     default:
         // Terrain failsafe recovery has failed, terrain data is not available
         // and rangefinder is not connected, or has stopped responding
-        gcs_send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
         failsafe_terrain_act();
         rangefinder_recovery_ms = 0;
         return;
@@ -728,7 +728,7 @@ void Sub::auto_terrain_recover_run()
     // exit on failure (timeout)
     if (AP_HAL::millis() > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {
         // Recovery has failed, revert to failsafe action
-        gcs_send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!");
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!");
         failsafe_terrain_act();
     }
 

ArduSub/control_surface.cpp

@@ -5,7 +5,7 @@ bool Sub::surface_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
     if (!ap.depth_sensor_present || failsafe.sensor_health) { // can't hold depth without a depth sensor, exit immediately.
-        gcs_send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
+        gcs().send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
         return false;
     }
 #endif

ArduSub/failsafe.cpp

@@ -82,7 +82,7 @@ void Sub::failsafe_sensors_check(void)
     }
 
     failsafe.sensor_health = true;
-    gcs_send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
+    gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_SENSORS, ERROR_CODE_BAD_DEPTH);
 
     if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
@@ -139,7 +139,7 @@ void Sub::failsafe_ekf_check(void)
 
     if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
         failsafe.last_ekf_warn_ms = AP_HAL::millis();
-        gcs_send_text(MAV_SEVERITY_WARNING, "EKF bad");
+        gcs().send_text(MAV_SEVERITY_WARNING, "EKF bad");
     }
 
     if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) {
@@ -167,7 +167,7 @@ void Sub::failsafe_battery_check(void)
     // Always warn when failsafe condition is met
     if (AP_HAL::millis() > failsafe.last_battery_warn_ms + 20000) {
         failsafe.last_battery_warn_ms = AP_HAL::millis();
-        gcs_send_text(MAV_SEVERITY_WARNING, "Low battery");
+        gcs().send_text(MAV_SEVERITY_WARNING, "Low battery");
     }
 
     // Don't do anything if failsafe has already been set
@@ -214,7 +214,7 @@ void Sub::failsafe_pilot_input_check()
     failsafe.pilot_input = true;
 
     Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
-    gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
+    gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
 
     if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
         set_neutral_controls();
@@ -251,7 +251,7 @@ void Sub::failsafe_internal_pressure_check()
     // Warn every 30 seconds
     if (failsafe.internal_pressure && tnow > last_pressure_warn_ms + 30000) {
         last_pressure_warn_ms = tnow;
-        gcs_send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!");
+        gcs().send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!");
     }
 }
 
@@ -283,7 +283,7 @@ void Sub::failsafe_internal_temperature_check()
     // Warn every 30 seconds
     if (failsafe.internal_temperature && tnow > last_temperature_warn_ms + 30000) {
         last_temperature_warn_ms = tnow;
-        gcs_send_text(MAV_SEVERITY_WARNING, "Internal temperature critical!");
+        gcs().send_text(MAV_SEVERITY_WARNING, "Internal temperature critical!");
     }
 }
 
@@ -309,7 +309,7 @@ void Sub::failsafe_leak_check()
     // Always send a warning every 20 seconds
     if (tnow > failsafe.last_leak_warn_ms + 20000) {
         failsafe.last_leak_warn_ms = tnow;
-        gcs_send_text(MAV_SEVERITY_CRITICAL, "Leak Detected");
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Leak Detected");
     }
 
     // Do nothing if we have already triggered the failsafe action, or if the motors are disarmed
@@ -355,7 +355,7 @@ void Sub::failsafe_gcs_check()
     // Send a warning every 30 seconds
     if (tnow > failsafe.last_gcs_warn_ms + 30000) {
         failsafe.last_gcs_warn_ms = tnow;
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs);
+        gcs().send_text(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs);
     }
 
     // do nothing if we have already triggered the failsafe action, or if the motors are disarmed
@@ -418,7 +418,7 @@ void Sub::failsafe_crash_check()
     // Send warning to GCS
     if (tnow > failsafe.last_crash_warn_ms + 20000) {
         failsafe.last_crash_warn_ms = tnow;
-        gcs_send_text(MAV_SEVERITY_WARNING,"Crash detected");
+        gcs().send_text(MAV_SEVERITY_WARNING,"Crash detected");
     }
 
     // Only perform failsafe action once
@@ -448,7 +448,7 @@ void Sub::failsafe_terrain_check()
     // check for clearing of event
     if (trigger_event != failsafe.terrain) {
         if (trigger_event) {
-            gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered");
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered");
             failsafe_terrain_on_event();
         } else {
             Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED);

ArduSub/joystick.cpp

@@ -204,10 +204,10 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             video_toggle = !video_toggle;
             if (video_toggle) {
                 video_switch = 1900;
-                gcs_send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2");
+                gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2");
             } else {
                 video_switch = 1100;
-                gcs_send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1");
+                gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1");
             }
         }
         break;
@@ -272,7 +272,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             } else {
                 gain = 1.0f;
             }
-            gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100);
+            gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100);
         }
         break;
     case JSButton::button_function_t::k_gain_inc:
@@ -288,7 +288,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
                 gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
             }
 
-            gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
+            gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
         }
         break;
     case JSButton::button_function_t::k_gain_dec:
@@ -304,7 +304,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
                 gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
             }
 
-            gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
+            gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
         }
         break;
     case JSButton::button_function_t::k_trim_roll_inc:
@@ -324,7 +324,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             zTrim = z_last-500;
             xTrim = x_last;
             yTrim = y_last;
-            gcs_send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
+            gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
         }
         break;
     case JSButton::button_function_t::k_relay_1_on:

ArduSub/motors.cpp

@@ -42,7 +42,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
     }
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    gcs_send_text(MAV_SEVERITY_INFO, "Arming motors");
+    gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
 #endif
 
     initial_armed_bearing = ahrs.yaw_sensor;
@@ -96,7 +96,7 @@ void Sub::init_disarm_motors()
     }
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
+    gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors");
 #endif
 
     // save compass offsets learned by the EKF if enabled

ArduSub/sensors.cpp

@@ -2,9 +2,9 @@
 
 void Sub::init_barometer(bool save)
 {
-    gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
+    gcs().send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
     barometer.calibrate(save);
-    gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
+    gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
 }
 
 // return barometric altitude in centimeters

ArduSub/turn_counter.cpp

@@ -58,7 +58,7 @@ void Sub::update_turn_counter()
     uint32_t tnow = AP_HAL::millis();
     if (quarter_turn_count/4 != last_turn_count_printed  && tnow > last_turn_announce_ms + 2000) {
         last_turn_announce_ms = tnow;
-        gcs_send_text_fmt(MAV_SEVERITY_INFO,"Tether is turned %i turns %s",int32_t(abs(quarter_turn_count)/4),(quarter_turn_count>0)?"to the right":"to the left");
+        gcs().send_text(MAV_SEVERITY_INFO,"Tether is turned %i turns %s",int32_t(abs(quarter_turn_count)/4),(quarter_turn_count>0)?"to the right":"to the left");
         last_turn_count_printed = quarter_turn_count/4;
     }
     last_turn_state = turn_state;