diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index c6172e925c..816c6e160b 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -378,7 +378,7 @@ void Plane::one_second_loop() void Plane::log_perf_info() { if (scheduler.debug() != 0) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "PERF: %u/%u Dt=%u/%u Log=%u", + gcs().send_text(MAV_SEVERITY_INFO, "PERF: %u/%u Dt=%u/%u Log=%u", (unsigned)perf.num_long, (unsigned)perf.mainLoop_count, (unsigned)perf.G_Dt_max, @@ -702,7 +702,7 @@ void Plane::update_flight_mode(void) if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) { if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) { auto_state.fbwa_tdrag_takeoff_mode = true; - gcs_send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); + gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); } } } @@ -888,7 +888,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND); if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", + gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100); } @@ -960,7 +960,7 @@ void Plane::update_flight_stage(void) // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); } else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90) { - plane.gcs_send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); + gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); } else { set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); @@ -1032,7 +1032,7 @@ void Plane::disarm_if_autoland_complete() /* we have auto disarm enabled. See if enough time has passed */ if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) { if (disarm_motors()) { - gcs_send_text(MAV_SEVERITY_INFO,"Auto disarmed"); + gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed"); } } } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 6ef5d47701..dd46fab474 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1403,7 +1403,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; if (!plane.geofence_present()) { - plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Fence not configured"); + gcs().send_text(MAV_SEVERITY_NOTICE,"Fence not configured"); result = MAV_RESULT_FAILED; } else { switch((uint16_t)packet.param1) { @@ -1421,7 +1421,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) if (! plane.geofence_set_floor_enabled(false)) { result = MAV_RESULT_FAILED; } else { - plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled"); + gcs().send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled"); } break; default: @@ -1464,7 +1464,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) plane.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); result = MAV_RESULT_ACCEPTED; - plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", + gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat*1.0e-7f), (double)(new_home_loc.lng*1.0e-7f), (uint32_t)(new_home_loc.alt*0.01f)); @@ -1497,10 +1497,10 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) case PARACHUTE_RELEASE: // treat as a manual release which performs some additional check of altitude if (plane.parachute.released()) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Parachute already released"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute already released"); result = MAV_RESULT_FAILED; } else if (!plane.parachute.enabled()) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Parachute not enabled"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute not enabled"); result = MAV_RESULT_FAILED; } else { if (!plane.parachute_manual_release()) { @@ -2026,7 +2026,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) plane.home_is_set = HOME_SET_NOT_LOCKED; plane.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); - plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", + gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat*1.0e-7f), (double)(new_home_loc.lng*1.0e-7f), (uint32_t)(new_home_loc.alt*0.01f)); @@ -2073,7 +2073,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) cmd.content.location.flags.terrain_alt = true; break; default: - plane.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT"); + gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT"); msg_valid = false; break; } @@ -2127,7 +2127,7 @@ void Plane::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); @@ -2166,26 +2166,6 @@ void Plane::gcs_update(void) gcs().update(); } -void Plane::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 Plane::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); - hal.util->vsnprintf((char *)str, sizeof(str), fmt, arg_list); - va_end(arg_list); - gcs().send_statustext(severity, 0xFF, str); -} - /* send airspeed calibration data */ diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 3784e34940..4e68dcb013 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -148,9 +148,9 @@ int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) void Plane::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"); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5f6b5abf3b..902361cdd0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -826,7 +826,6 @@ private: void gcs_send_mission_item_reached_message(uint16_t mission_index); void gcs_data_stream_send(void); void gcs_update(void); - void gcs_send_text(MAV_SEVERITY severity, const char *str); void gcs_send_airspeed_calibration(const Vector3f &vg); void gcs_retry_deferred(void); @@ -1040,7 +1039,6 @@ private: void update_is_flying_5Hz(void); void crash_detection_update(void); bool in_preLaunch_flight_stage(void); - void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); void handle_auto_mode(void); void calc_throttle(); void calc_nav_roll(); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 0ecbfa91e1..08b32b6c11 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -606,7 +606,7 @@ void Plane::rangefinder_height_update(void) (control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) && g.rangefinder_landing) { rangefinder_state.in_use = true; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate); + gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate); } } rangefinder_state.last_distance = distance; @@ -643,7 +643,7 @@ void Plane::rangefinder_height_update(void) if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) { // the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar if (rangefinder_state.in_use) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)rangefinder_state.height_estimate); + gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)rangefinder_state.height_estimate); } memset(&rangefinder_state, 0, sizeof(rangefinder_state)); } diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 59b93ec5a0..c4b00b9a30 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -51,7 +51,7 @@ void Plane::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_loc, next_WP_loc)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint"); prev_WP_loc = current_loc; } @@ -104,7 +104,7 @@ void Plane::set_guided_WP(void) // ------------------------------- void Plane::init_home() { - gcs_send_text(MAV_SEVERITY_INFO, "Init HOME"); + gcs().send_text(MAV_SEVERITY_INFO, "Init HOME"); ahrs.set_home(gps.location()); home_is_set = HOME_SET_NOT_LOCKED; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index bf1856da35..240f363922 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -34,9 +34,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) const uint16_t next_index = mission.get_current_nav_index() + 1; auto_state.wp_is_land_approach = mission.get_next_nav_cmd(next_index, next_nav_cmd) && (next_nav_cmd.id == MAV_CMD_NAV_LAND); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing nav command ID #%i",cmd.id); + gcs().send_text(MAV_SEVERITY_INFO, "Executing nav command ID #%i",cmd.id); } else { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing command ID #%i",cmd.id); + gcs().send_text(MAV_SEVERITY_INFO, "Executing command ID #%i",cmd.id); } switch(cmd.id) { @@ -131,7 +131,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_INVERTED_FLIGHT: if (cmd.p1 == 0 || cmd.p1 == 1) { auto_state.inverted_flight = (bool)cmd.p1; - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set inverted %u", cmd.p1); + gcs().send_text(MAV_SEVERITY_INFO, "Set inverted %u", cmd.p1); } break; @@ -142,15 +142,15 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) #if GEOFENCE_ENABLED == ENABLED if (cmd.p1 != 2) { if (!geofence_set_enabled((bool) cmd.p1, AUTO_TOGGLED)) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to set fence. Enabled state to %u", cmd.p1); + gcs().send_text(MAV_SEVERITY_WARNING, "Unable to set fence. Enabled state to %u", cmd.p1); } else { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set fence enabled state to %u", cmd.p1); + gcs().send_text(MAV_SEVERITY_INFO, "Set fence enabled state to %u", cmd.p1); } } else { //commanding to only disable floor if (! geofence_set_floor_enabled(false)) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to disable fence floor"); + gcs().send_text(MAV_SEVERITY_WARNING, "Unable to disable fence floor"); } else { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Fence floor disabled"); + gcs().send_text(MAV_SEVERITY_WARNING, "Fence floor disabled"); } } #endif @@ -318,7 +318,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret 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; } @@ -409,15 +409,15 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable == 1) { if (! geofence_set_enabled(false, AUTO_TOGGLED)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)"); } else { - gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)"); } } else if (g.fence_autoenable == 2) { if (! geofence_set_floor_enabled(false)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)"); } else { - gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); } } #endif @@ -532,7 +532,7 @@ bool Plane::verify_takeoff() float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err; takeoff_course = wrap_PI(takeoff_course); steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)", + gcs().send_text(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)", steer_state.hold_course_cd, (double)gps.ground_speed(), (double)degrees(steer_state.locked_course_err)); @@ -549,7 +549,7 @@ bool Plane::verify_takeoff() // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; auto_state.takeoff_complete = true; @@ -622,7 +622,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } if (auto_state.wp_distance <= acceptance_distance_m) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um", + gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um", (unsigned)mission.get_current_nav_cmd().index, (unsigned)get_distance(current_loc, flex_next_WP_loc)); return true; @@ -630,7 +630,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // have we flown past the waypoint? if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um", + gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um", (unsigned)mission.get_current_nav_cmd().index, (unsigned)get_distance(current_loc, flex_next_WP_loc)); return true; @@ -676,7 +676,7 @@ bool Plane::verify_loiter_time() } if (result) { - gcs_send_text(MAV_SEVERITY_INFO,"Loiter time complete"); + gcs().send_text(MAV_SEVERITY_INFO,"Loiter time complete"); auto_state.vtol_loiter = false; } return result; @@ -704,7 +704,7 @@ bool Plane::verify_loiter_turns() } if (result) { - gcs_send_text(MAV_SEVERITY_INFO,"Loiter orbits complete"); + gcs().send_text(MAV_SEVERITY_INFO,"Loiter orbits complete"); } return result; } @@ -725,7 +725,7 @@ bool Plane::verify_loiter_to_alt() if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) { // primary goal completed, initialize secondary heading goal if (loiter.unable_to_acheive_target_alt) { - gcs_send_text_fmt(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", current_loc.alt/100); + gcs().send_text(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", current_loc.alt/100); } condition_value = 1; @@ -737,7 +737,7 @@ bool Plane::verify_loiter_to_alt() } if (result) { - gcs_send_text(MAV_SEVERITY_INFO,"Loiter to alt complete"); + gcs().send_text(MAV_SEVERITY_INFO,"Loiter to alt complete"); } return result; } @@ -752,7 +752,7 @@ bool Plane::verify_RTL() update_loiter(abs(g.rtl_radius)); if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) || reached_loiter_target()) { - gcs_send_text(MAV_SEVERITY_INFO,"Reached RTL location"); + gcs().send_text(MAV_SEVERITY_INFO,"Reached RTL location"); return true; } else { return false; @@ -802,11 +802,11 @@ bool Plane::verify_continue_and_change_alt() bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) { if (current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) { - gcs_send_text(MAV_SEVERITY_INFO,"Reached altitude"); + gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude"); return true; } if (auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)auto_state.sink_rate); + gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)auto_state.sink_rate); return true; } @@ -882,17 +882,17 @@ void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) case 0: // Airspeed if (cmd.content.speed.target_ms > 0) { aparm.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); + gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); } break; case 1: // Ground speed - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); + gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); break; } if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); + gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); aparm.throttle_cruise.set(cmd.content.speed.throttle_pct); } } @@ -1018,7 +1018,7 @@ void Plane::exit_mission_callback() { if (control_mode == AUTO) { set_mode(RTL, MODE_REASON_MISSION_END); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL"); + gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL"); } } diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 926cb975ec..5696c78610 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -74,17 +74,17 @@ void Plane::read_control_switch() if (hal.util->get_soft_armed() || (last_mixer_crc != -1)) { px4io_override_enabled = true; // disable output channels to force PX4IO override - gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled"); + gcs().send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled"); } else { // we'll let the one second loop reconfigure the mixer. The // PX4IO code sometimes rejects a mixer, probably due to it // being busy in some way? - gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed"); + gcs().send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed"); } } else if (!override_requested && px4io_override_enabled) { px4io_override_enabled = false; SRV_Channels::enable_aux_servos(); - gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override disabled"); + gcs().send_text(MAV_SEVERITY_WARNING, "PX4IO override disabled"); } if (px4io_override_enabled && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED && diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index b01b68eee4..badf86ac51 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -5,7 +5,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re // This is how to handle a short loss of control signal failsafe. failsafe.state = fstype; failsafe.ch3_timer_ms = millis(); - gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on, "); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on, "); switch(control_mode) { case MANUAL: @@ -55,13 +55,13 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re default: break; } - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); + gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); } void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason) { // This is how to handle a long loss of control signal failsafe. - gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, "); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, "); // If the GCS is locked up we allow control to revert to RC hal.rcin->clear_overrides(); failsafe.state = fstype; @@ -115,15 +115,15 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea break; } if (fstype == FAILSAFE_GCS) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "No GCS heartbeat"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "No GCS heartbeat"); } - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); + gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); } void Plane::failsafe_short_off_event(mode_reason_t reason) { // We're back in radio contact - gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off"); + gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off"); failsafe.state = FAILSAFE_NONE; // re-read the switch so we can return to our preferred mode @@ -139,7 +139,7 @@ void Plane::low_battery_event(void) if (failsafe.low_battery) { return; } - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh", + gcs().send_text(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh", (double)battery.voltage(), (double)battery.current_total_mah()); if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE); diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index f6dcb135e4..eb0bef9c32 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -136,13 +136,13 @@ void Plane::geofence_load(void) geofence_state->boundary_uptodate = true; geofence_state->fence_triggered = false; - gcs_send_text(MAV_SEVERITY_INFO,"Geofence loaded"); + gcs().send_text(MAV_SEVERITY_INFO,"Geofence loaded"); gcs_send_message(MSG_FENCE_STATUS); return; failed: g.fence_action.set(FENCE_ACTION_NONE); - gcs_send_text(MAV_SEVERITY_WARNING,"Geofence setup error"); + gcs().send_text(MAV_SEVERITY_WARNING,"Geofence setup error"); } /* @@ -334,7 +334,7 @@ void Plane::geofence_check(bool altitude_check_only) if (geofence_state->fence_triggered && !altitude_check_only) { // we have moved back inside the fence geofence_state->fence_triggered = false; - gcs_send_text(MAV_SEVERITY_INFO,"Geofence OK"); + gcs().send_text(MAV_SEVERITY_INFO,"Geofence OK"); #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 0); @@ -364,7 +364,7 @@ void Plane::geofence_check(bool altitude_check_only) hal.gpio->write(FENCE_TRIGGERED_PIN, 1); #endif - gcs_send_text(MAV_SEVERITY_NOTICE,"Geofence triggered"); + gcs().send_text(MAV_SEVERITY_NOTICE,"Geofence triggered"); gcs_send_message(MSG_FENCE_STATUS); // see what action the user wants diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 23bf3c4123..7d9f40b6d3 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -280,9 +280,9 @@ void Plane::crash_detection_update(void) if (aparm.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); } else { - gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken"); + gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken"); } } else { @@ -290,9 +290,9 @@ void Plane::crash_detection_update(void) disarm_motors(); } if (crashed_near_land_waypoint) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); } else { - gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected"); + gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected"); } } } diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index a139a0856b..529f83b1a5 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -78,7 +78,7 @@ uint8_t QuadPlane::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t moto uint16_t throttle_value, float timeout_sec, uint8_t motor_count) { if (motors->armed()) { - plane.gcs_send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test"); + gcs().send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test"); return MAV_RESULT_FAILED; } // if test has not started try to start it diff --git a/ArduPlane/parachute.cpp b/ArduPlane/parachute.cpp index 0cbd41c973..ba6bdf7aea 100644 --- a/ArduPlane/parachute.cpp +++ b/ArduPlane/parachute.cpp @@ -23,7 +23,7 @@ void Plane::parachute_release() } // send message to gcs and dataflash - gcs_send_text(MAV_SEVERITY_CRITICAL,"Parachute: Released"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Parachute: Released"); // release parachute parachute.release(); @@ -44,7 +44,7 @@ bool Plane::parachute_manual_release() if (parachute.alt_min() > 0 && relative_ground_altitude(false) < parachute.alt_min() && auto_state.last_flying_ms > 0) { // Allow manual ground tests by only checking if flying too low if we've taken off - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Parachute: Too low"); + gcs().send_text(MAV_SEVERITY_WARNING, "Parachute: Too low"); return false; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 27b75c92fc..dd9fb39828 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1545,24 +1545,24 @@ bool QuadPlane::init_mode(void) bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) { if (!available()) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL not available"); + gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL not available"); return false; } if (plane.control_mode != AUTO) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO"); + gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO"); return false; } switch (state) { case MAV_VTOL_STATE_MC: if (!plane.auto_state.vtol_mode) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); } plane.auto_state.vtol_mode = true; return true; case MAV_VTOL_STATE_FW: if (plane.auto_state.vtol_mode) { - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Exited VTOL mode"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Exited VTOL mode"); } plane.auto_state.vtol_mode = false; @@ -1572,7 +1572,7 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) break; } - plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Invalid VTOL mode"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Invalid VTOL mode"); return false; } @@ -1740,7 +1740,7 @@ void QuadPlane::vtol_position_controller(void) plane.auto_state.wp_distance < 5) { poscontrol.state = QPOS_POSITION2; wp_nav->init_loiter_target(); - plane.gcs_send_text_fmt(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f", + gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f", (double)ahrs.groundspeed(), (double)plane.auto_state.wp_distance); } break; @@ -2085,7 +2085,7 @@ void QuadPlane::check_land_complete(void) // change in altitude for last 4s. We are landed. plane.disarm_motors(); poscontrol.state = QPOS_LAND_COMPLETE; - plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete"); + gcs().send_text(MAV_SEVERITY_INFO,"Land complete"); // reload target airspeed which could have been modified by the mission plane.aparm.airspeed_cruise_cm.load(); } @@ -2101,7 +2101,7 @@ bool QuadPlane::verify_vtol_land(void) if (poscontrol.state == QPOS_POSITION2 && plane.auto_state.wp_distance < 2) { poscontrol.state = QPOS_LAND_DESCEND; - plane.gcs_send_text(MAV_SEVERITY_INFO,"Land descend started"); + gcs().send_text(MAV_SEVERITY_INFO,"Land descend started"); plane.set_next_WP(plane.next_WP_loc); } @@ -2119,7 +2119,7 @@ bool QuadPlane::verify_vtol_land(void) if (land_icengine_cut != 0) { plane.g2.ice_control.engine_control(0, 0, 0); } - plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started"); + gcs().send_text(MAV_SEVERITY_INFO,"Land final started"); } check_land_complete(); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index f5b14ed0ea..d18416714f 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -266,7 +266,7 @@ void Plane::control_failsafe(uint16_t pwm) // throttle has dropped below the mark failsafe.ch3_counter++; if (failsafe.ch3_counter == 10) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Throttle failsafe on %u", (unsigned)pwm); + gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on %u", (unsigned)pwm); failsafe.ch3_failsafe = true; AP_Notify::flags.failsafe_radio = true; } @@ -282,7 +282,7 @@ void Plane::control_failsafe(uint16_t pwm) failsafe.ch3_counter = 3; } if (failsafe.ch3_counter == 1) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Throttle failsafe off %u", (unsigned)pwm); + gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off %u", (unsigned)pwm); } else if(failsafe.ch3_counter == 0) { failsafe.ch3_failsafe = false; AP_Notify::flags.failsafe_radio = false; diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 19bb5d9a8b..42a8853f5e 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -3,13 +3,13 @@ void Plane::init_barometer(bool full_calibration) { - gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer"); + gcs().send_text(MAV_SEVERITY_INFO, "Calibrating barometer"); if (full_calibration) { barometer.calibrate(); } else { barometer.update_calibration(); } - gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); + gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); } void Plane::init_rangefinder(void) @@ -108,7 +108,7 @@ void Plane::zero_airspeed(bool in_startup) read_airspeed(); // update barometric calibration with new airspeed supplied temperature barometer.update_calibration(); - gcs_send_text(MAV_SEVERITY_INFO,"Airspeed calibration started"); + gcs().send_text(MAV_SEVERITY_INFO,"Airspeed calibration started"); } // read_battery - reads battery voltage and current and invokes failsafe diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 10248db22a..47f27ac9e0 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -279,10 +279,10 @@ void Plane::startup_ground(void) set_mode(INITIALISING, MODE_REASON_UNKNOWN); #if (GROUND_START_DELAY > 0) - gcs_send_text(MAV_SEVERITY_NOTICE,"Ground start with delay"); + gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay"); delay(GROUND_START_DELAY * 1000); #else - gcs_send_text(MAV_SEVERITY_INFO,"Ground start"); + gcs().send_text(MAV_SEVERITY_INFO,"Ground start"); #endif //INS ground start @@ -318,7 +318,7 @@ void Plane::startup_ground(void) // ready to fly serial_manager.set_blocking_writes_all(false); - gcs_send_text(MAV_SEVERITY_INFO,"Ground start complete"); + gcs().send_text(MAV_SEVERITY_INFO,"Ground start complete"); } enum FlightMode Plane::get_previous_mode() { @@ -636,14 +636,14 @@ void Plane::startup_INS_ground(void) while (barometer.get_last_update() == 0) { // the barometer begins updating when we get the first // HIL_STATE message - gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); + gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); hal.scheduler->delay(1000); } } #endif if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { - gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane"); + gcs().send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane"); hal.scheduler->delay(100); } @@ -664,7 +664,7 @@ void Plane::startup_INS_ground(void) // -------------------------- zero_airspeed(true); } else { - gcs_send_text(MAV_SEVERITY_WARNING,"No airspeed"); + gcs().send_text(MAV_SEVERITY_WARNING,"No airspeed"); } } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index e3d12206ab..73e807058d 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -42,7 +42,7 @@ bool Plane::auto_takeoff_check(void) takeoff_state.launchTimerStarted = true; takeoff_state.last_tkoff_arm_time = now; if (now - takeoff_state.last_report_ms > 2000) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec", + gcs().send_text(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec", (double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f)); takeoff_state.last_report_ms = now; } @@ -51,7 +51,7 @@ bool Plane::auto_takeoff_check(void) // Only perform velocity check if not timed out if ((now - takeoff_state.last_tkoff_arm_time) > wait_time_ms+100U) { if (now - takeoff_state.last_report_ms > 2000) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timeout AUTO"); + gcs().send_text(MAV_SEVERITY_WARNING, "Timeout AUTO"); takeoff_state.last_report_ms = now; } goto no_launch; @@ -62,7 +62,7 @@ bool Plane::auto_takeoff_check(void) if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 || (!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Bad launch AUTO"); + gcs().send_text(MAV_SEVERITY_WARNING, "Bad launch AUTO"); goto no_launch; } } @@ -70,7 +70,7 @@ bool Plane::auto_takeoff_check(void) // Check ground speed and time delay if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) && ((now - takeoff_state.last_tkoff_arm_time) >= wait_time_ms)) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed()); + gcs().send_text(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed()); takeoff_state.launchTimerStarted = false; takeoff_state.last_tkoff_arm_time = 0; steer_state.locked_course_err = 0; // use current heading without any error offset @@ -186,7 +186,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) relative_alt_cm >= 1000 && sec_to_target <= g.takeoff_pitch_limit_reduction_sec) { // make a note of that altitude to use it as a start height for scaling - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100); + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100); auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm; } } @@ -231,7 +231,7 @@ int8_t Plane::takeoff_tail_hold(void) return_zero: if (auto_state.fbwa_tdrag_takeoff_mode) { - gcs_send_text(MAV_SEVERITY_NOTICE, "FBWA tdrag off"); + gcs().send_text(MAV_SEVERITY_NOTICE, "FBWA tdrag off"); auto_state.fbwa_tdrag_takeoff_mode = false; } return 0; @@ -246,9 +246,9 @@ void Plane::complete_auto_takeoff(void) #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable > 0) { if (! geofence_set_enabled(true, AUTO_TOGGLED)) { - gcs_send_text(MAV_SEVERITY_NOTICE, "Enable fence failed (cannot autoenable"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Enable fence failed (cannot autoenable"); } else { - gcs_send_text(MAV_SEVERITY_INFO, "Fence enabled (autoenabled)"); + gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled (autoenabled)"); } } #endif