Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Correct compilation when GCS library not available #25730

Merged
merged 10 commits into from
Dec 12, 2023
Merged
3 changes: 2 additions & 1 deletion libraries/AC_AttitudeControl/AC_WeatherVane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -219,7 +219,8 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
}

if (!active_msg_sent) {
gcs().send_text(MAV_SEVERITY_INFO, "Weathervane Active: %s", dir_string);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Weathervane Active: %s", dir_string);
(void)dir_string; // in case GCS is disabled
active_msg_sent = true;
}

Expand Down
10 changes: 7 additions & 3 deletions libraries/AP_Arming/AP_Arming.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -244,6 +244,7 @@ void AP_Arming::check_failed(const enum AP_Arming::ArmingChecks check, bool repo
}
hal.util->snprintf(taggedfmt, sizeof(taggedfmt), metafmt, fmt);

#if HAL_GCS_ENABLED
MAV_SEVERITY severity = MAV_SEVERITY_CRITICAL;
if (!check_enabled(check)) {
// technically should be NOTICE, but will annoy users at that level:
Expand All @@ -253,10 +254,12 @@ void AP_Arming::check_failed(const enum AP_Arming::ArmingChecks check, bool repo
va_start(arg_list, fmt);
gcs().send_textv(severity, taggedfmt, arg_list);
va_end(arg_list);
#endif // HAL_GCS_ENABLED
}

void AP_Arming::check_failed(bool report, const char *fmt, ...) const
{
#if HAL_GCS_ENABLED
if (!report) {
return;
}
Expand All @@ -275,6 +278,7 @@ void AP_Arming::check_failed(bool report, const char *fmt, ...) const
va_start(arg_list, fmt);
gcs().send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list);
va_end(arg_list);
#endif // HAL_GCS_ENABLED
}

bool AP_Arming::barometer_checks(bool report)
Expand Down Expand Up @@ -1656,7 +1660,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks)
running_arming_checks = false;

if (armed && do_arming_checks && checks_to_perform == 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled");
}

#if HAL_GYROFFT_ENABLED
Expand Down Expand Up @@ -1685,7 +1689,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks)
// If a fence is set to auto-enable, turn on the fence
if (fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
fence->enable(true);
gcs().send_text(MAV_SEVERITY_INFO, "Fence: auto-enabled");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fence: auto-enabled");
}
}
}
Expand Down Expand Up @@ -1752,7 +1756,7 @@ void AP_Arming::send_arm_disarm_statustext(const char *str) const
if (option_enabled(AP_Arming::Option::DISABLE_STATUSTEXT_ON_STATE_CHANGE)) {
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "%s", str);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", str);
}

AP_Arming::Required AP_Arming::arming_required() const
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Avoidance/AP_Avoidance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -396,6 +396,7 @@ MAV_COLLISION_THREAT_LEVEL AP_Avoidance::current_threat_level() const {
return _obstacles[_current_most_serious_threat].threat_level;
}

#if HAL_GCS_ENABLED
void AP_Avoidance::send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour) const
{
const mavlink_collision_t packet{
Expand All @@ -409,6 +410,7 @@ void AP_Avoidance::send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_COLLISION, (const char *)&packet);
}
#endif

void AP_Avoidance::handle_threat_gcs_notify(AP_Avoidance::Obstacle *threat)
{
Expand Down
10 changes: 5 additions & 5 deletions libraries/AP_Landing/AP_Landing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -253,7 +253,7 @@ bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc,
default:
// returning TRUE while executing verify_land() will increment the
// mission index which in many cases will trigger an RTL for end-of-mission
gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE");
success = true;
break;
}
Expand Down Expand Up @@ -486,25 +486,25 @@ bool AP_Landing::restart_landing_sequence()
mission.set_current_cmd(current_index+1))
{
// if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it
gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", (signed)cmd.content.location.alt/100);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", (signed)cmd.content.location.alt/100);
success = true;
}
else if (do_land_start_index != 0 &&
mission.set_current_cmd(do_land_start_index))
{
// look for a DO_LAND_START and use that index
gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
success = true;
}
else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
mission.set_current_cmd(prev_cmd_with_wp_index))
{
// if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
// repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
success = true;
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
success = false;
}

Expand Down
20 changes: 10 additions & 10 deletions libraries/AP_Landing/AP_Landing_Deepstall.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -345,7 +345,7 @@ bool AP_Landing_Deepstall::override_servos(void)

if (elevator == nullptr) {
// deepstalls are impossible without these channels, abort the process
gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels");
request_go_around();
return false;
}
Expand Down Expand Up @@ -533,17 +533,17 @@ void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)

#ifdef DEBUG_PRINTS
// TODO: Send this information via a MAVLink packet
gcs().send_text(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
(double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
gcs().send_text(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
(double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
gcs().send_text(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
(double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
gcs().send_text(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
(double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
gcs().send_text(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
(double)expected_travel_distance);
gcs().send_text(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}
Expand Down Expand Up @@ -590,7 +590,7 @@ float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const f

if(print) {
// allow printing the travel distances on the final entry as its used for tuning
gcs().send_text(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)",
(double)stall_distance, (double)predicted_travel_distance);
}

Expand Down Expand Up @@ -620,7 +620,7 @@ float AP_Landing_Deepstall::update_steering()
// panic if no position source is available
// continue the stall but target just holding the wings held level as deepstall should be a minimal
// energy configuration on the aircraft, and if a position isn't available aborting would be worse
gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
hold_level = true;
}

Expand Down Expand Up @@ -652,7 +652,7 @@ float AP_Landing_Deepstall::update_steering()
float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);

#ifdef DEBUG_PRINTS
gcs().send_text(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
(double)crosstrack_error,
(double)error,
(double)degrees(yaw_rate),
Expand Down
14 changes: 7 additions & 7 deletions libraries/AP_Landing/AP_Landing_Slope.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons
type_slope_flags.post_stats = false;

type_slope_stage = SlopeStage::NORMAL;
gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
}

void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
Expand Down Expand Up @@ -106,9 +106,9 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
if (type_slope_stage != SlopeStage::FINAL) {
type_slope_flags.post_stats = true;
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
(double)height, (double)sink_rate,
(double)gps.ground_speed(),
(double)current_loc.get_distance(next_WP_loc));
Expand All @@ -122,7 +122,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
AP_LandingGear *LG_inst = AP_LandingGear::get_singleton();
if (LG_inst != nullptr && !LG_inst->check_before_land()) {
type_slope_request_go_around();
gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed");
}
#endif
}
Expand Down Expand Up @@ -158,7 +158,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
// this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
if (type_slope_flags.post_stats && !is_armed) {
type_slope_flags.post_stats = false;
gcs().send_text(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)current_loc.get_distance(next_WP_loc));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)current_loc.get_distance(next_WP_loc));
}

// check if we should auto-disarm after a confirmed landing
Expand Down Expand Up @@ -226,7 +226,7 @@ void AP_Landing::type_slope_adjust_landing_slope_for_rangefinder_bump(AP_FixedWi

// is projected slope too steep?
if (new_slope_deg - initial_slope_deg > slope_recalc_steep_threshold_to_abort) {
gcs().send_text(MAV_SEVERITY_INFO, "Landing slope too steep, aborting (%.0fm %.1fdeg)",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing slope too steep, aborting (%.0fm %.1fdeg)",
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
alt_offset = rangefinder_state.correction;
flags.commanded_go_around = true;
Expand Down Expand Up @@ -319,7 +319,7 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
bool is_first_calc = is_zero(slope);
slope = (sink_height - aim_height) / (total_distance - flare_distance);
if (is_first_calc) {
gcs().send_text(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope)));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope)));
}

// calculate point along that slope 500m ahead
Expand Down
16 changes: 8 additions & 8 deletions libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -313,20 +313,20 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
_idle_throttle = constrain_float( get_arot_idle_output(), 0.0f, 0.4f);
}
if (!_autorotating) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Autorotation");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Autorotation");
_autorotating =true;
}
} else {
if (_autorotating) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Autorotation Stopped");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Autorotation Stopped");
_autorotating =false;
}
// set rotor control speed to idle speed parameter, this happens instantly and ignores ramping
if (_turbine_start && _starting == true ) {
_idle_throttle += 0.001f;
if (_control_output >= 1.0f) {
_idle_throttle = get_idle_output();
gcs().send_text(MAV_SEVERITY_INFO, "Turbine startup");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turbine startup");
_starting = false;
}
} else {
Expand Down Expand Up @@ -408,7 +408,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt)
if (_rotor_ramp_output < rotor_ramp_input) {
if (_use_bailout_ramp) {
if (!_bailing_out) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "bailing_out");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "bailing_out");
_bailing_out = true;
if (_control_mode == ROTOR_CONTROL_MODE_AUTOTHROTTLE) {_gov_bailing_out = true;}
}
Expand Down Expand Up @@ -563,9 +563,9 @@ void AP_MotorsHeli_RSC::autothrottle_run()
governor_reset();
_governor_fault = true;
if (_rotor_rpm >= (_governor_rpm + _governor_range)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed");
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Underspeed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Underspeed");
}
}
} else {
Expand All @@ -577,7 +577,7 @@ void AP_MotorsHeli_RSC::autothrottle_run()
if (_rotor_rpm > (_governor_rpm + _governor_range) && !_gov_bailing_out) {
_governor_fault = true;
governor_reset();
gcs().send_text(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed");
_governor_output = 0.0f;

// when performing power recovery from autorotation, this waits for user to load rotor in order to
Expand All @@ -594,7 +594,7 @@ void AP_MotorsHeli_RSC::autothrottle_run()
_governor_engage = true;
_autothrottle = true;
_gov_bailing_out = false;
gcs().send_text(MAV_SEVERITY_NOTICE, "Governor Engaged");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Governor Engaged");
}
} else {
// temporary use of throttle curve and ramp timer to accelerate rotor to governor min torque rise speed
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,12 @@ bool AP_OSD_MSP_DisplayPort::init(void)
// check if we have a DisplayPort backend to use
const AP_MSP *msp = AP::msp();
if (msp == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"MSP backend not available");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP backend not available");
return false;
}
_displayport = msp->find_protocol(AP_SerialManager::SerialProtocol_MSP_DisplayPort);
if (_displayport == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available");
return false;
}
// re-init port here for use in this thread
Expand Down
6 changes: 3 additions & 3 deletions libraries/AP_SmartRTL/AP_SmartRTL.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ void AP_SmartRTL::init()
// check if memory allocation failed
if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) {
log_action(SRTL_DEACTIVATED_INIT_FAILED);
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed");
free(_path);
free(_prune.loops);
free(_simplify.stack);
Expand Down Expand Up @@ -390,7 +390,7 @@ void AP_SmartRTL::run_background_cleanup()
// warn if buffer is about to be filled
uint32_t now_ms = AP_HAL::millis();
if ((path_points_count >0) && (path_points_count >= _path_points_max - 9) && (now_ms - _last_low_space_notify_ms > 10000)) {
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL Low on space!");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SmartRTL Low on space!");
_last_low_space_notify_ms = now_ms;
}

Expand Down Expand Up @@ -866,7 +866,7 @@ void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason)
{
_active = false;
log_action(action);
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason);
}

// logging
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ bool AP_VisualOdom_Backend::healthy() const
return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
}

#if HAL_GCS_ENABLED
// consume vision_position_delta mavlink messages
void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_message_t &msg)
{
Expand Down Expand Up @@ -69,6 +70,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa
position_delta,
packet.confidence);
}
#endif // HAL_GCS_ENABLED

// returns the system time of the last reset if reset_counter has not changed
// updates the reset timestamp to the current system time if the reset_counter has changed
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,10 @@ class AP_VisualOdom_Backend
// return true if sensor is basically healthy (we are receiving data)
bool healthy() const;

#if HAL_GCS_ENABLED
// consume vision_position_delta mavlink messages
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
#endif

// consume vision pose estimate data and send to EKF. distances in meters
virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;
Expand Down
Loading