diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 592b9d774d..193f6276c8 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/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; diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index e71cb46c06..883a324f93 100644 --- a/ArduSub/ArduSub.cpp +++ b/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(), diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 20d274eeee..37284c955b 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/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); -} diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 3dd9ae2ad3..2e158da2cd 100644 --- a/ArduSub/Log.cpp +++ b/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 diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 757552cf6e..d662aeb811 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index db3d7823fa..aafccddce3 100644 --- a/ArduSub/commands_logic.cpp +++ b/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; diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index aaf0572153..2f27450128 100644 --- a/ArduSub/control_althold.cpp +++ b/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 diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index d79a0c531c..6f061f480f 100644 --- a/ArduSub/control_auto.cpp +++ b/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(); } diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index 1fe03b1c70..a211e686e7 100644 --- a/ArduSub/control_surface.cpp +++ b/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 diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 35f7717e71..0cfcb858bf 100644 --- a/ArduSub/failsafe.cpp +++ b/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); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index b807aee3f7..bc6f5cf527 100644 --- a/ArduSub/joystick.cpp +++ b/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: diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 7cadc7e4a0..34fba256e1 100644 --- a/ArduSub/motors.cpp +++ b/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 diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 095b145b64..d4c31e7341 100644 --- a/ArduSub/sensors.cpp +++ b/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 diff --git a/ArduSub/turn_counter.cpp b/ArduSub/turn_counter.cpp index c257d8f3c2..dc20fe517f 100644 --- a/ArduSub/turn_counter.cpp +++ b/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;