diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index d6841ad4a9..d9e1677cda 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -43,7 +43,7 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure) // at the same time. This cannot be allowed. if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); } return false; } @@ -54,7 +54,7 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure) // as state can change at any time. if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); } return false; } @@ -101,7 +101,7 @@ bool AP_Arming_Copter::barometer_checks(bool display_failure) // barometer health check if (!barometer.all_healthy()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy"); } return false; } @@ -113,7 +113,7 @@ bool AP_Arming_Copter::barometer_checks(bool display_failure) if (using_baro_ref) { if (fabsf(_inav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); } return false; } @@ -131,7 +131,7 @@ bool AP_Arming_Copter::compass_checks(bool display_failure) // this if learning is off; Copter *always* checks. if (!_compass.configured()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated"); } ret = false; } @@ -173,7 +173,7 @@ bool AP_Arming_Copter::ins_checks(bool display_failure) // get ekf attitude (if bad, it's usually the gyro biases) if (!pre_arm_ekf_attitude_check()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling"); } ret = false; } @@ -189,7 +189,7 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure) if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage"); } return false; } @@ -202,7 +202,7 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure) if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { if (copter.failsafe.battery || (!copter.ap.usb_connected && copter.battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery"); } return false; } @@ -219,7 +219,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // ensure ch7 and ch8 have different functions if (copter.check_duplicate_auxsw()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options"); } return false; } @@ -229,7 +229,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); } return false; } @@ -238,7 +238,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // lean angle parameter check if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX"); } return false; } @@ -246,7 +246,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // acro balance parameter check if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); } return false; } @@ -255,7 +255,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // check range finder if optflow enabled if (copter.optflow.enabled() && !copter.rangefinder.pre_arm_check()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); } return false; } @@ -276,7 +276,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // check adsb avoidance failsafe if (copter.failsafe.adsb) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected"); } return false; } @@ -296,7 +296,7 @@ bool AP_Arming_Copter::motor_checks(bool display_failure) // check motors initialised correctly if (!copter.motors->initialised_ok()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check firmware or FRAME_CLASS"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check firmware or FRAME_CLASS"); } return false; } @@ -311,9 +311,9 @@ bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure) if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe"); #else - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe"); #endif } return false; @@ -351,19 +351,19 @@ void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure) // check if radio has been calibrated if (!channel->min_max_configured()) { if (display_failure) { - copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: RC %s not configured", channel_name); + copter.gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC %s not configured", channel_name); } return; } if (channel->get_radio_min() > 1300) { if (display_failure) { - copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name); + copter.gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name); } return; } if (channel->get_radio_max() < 1700) { if (display_failure) { - copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name); + copter.gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name); } return; } @@ -373,13 +373,13 @@ void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure) } if (channel->get_radio_trim() < channel->get_radio_min()) { if (display_failure) { - copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim below min", channel_name); + copter.gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim below min", channel_name); } return; } if (channel->get_radio_trim() > channel->get_radio_max()) { if (display_failure) { - copter.gcs_send_text_fmt(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim above max", channel_name); + copter.gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim above max", channel_name); } return; } @@ -395,7 +395,7 @@ bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure) // always check if inertial nav has started and is ready if (!ahrs.healthy()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks"); } return false; } @@ -425,9 +425,9 @@ bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure) } else { if (!mode_requires_gps && fence_requires_gps) { // clarify to user why they need GPS in non-GPS flight mode - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Fence enabled, need 3D Fix"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Fence enabled, need 3D Fix"); } else { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix"); } } } @@ -442,7 +442,7 @@ bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure) _ahrs_navekf.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); if (mag_variance.length() >= copter.g.fs_ekf_thresh) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance"); } return false; } @@ -450,7 +450,7 @@ bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure) // check home and EKF origin are not too far if (copter.far_from_EKF_origin(ahrs.get_home())) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance"); } AP_Notify::flags.pre_arm_gps_check = false; return false; @@ -465,7 +465,7 @@ bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure) // warn about hdop separately - to prevent user confusion with no gps lock if (copter.gps.get_hdop() > copter.g.gps_hdop_good) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); } AP_Notify::flags.pre_arm_gps_check = false; return false; @@ -503,7 +503,7 @@ bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure) // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check if (copter.rangefinder_state.enabled && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270))) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range"); return false; } @@ -512,7 +512,7 @@ bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure) copter.terrain.get_statistics(terr_pending, terr_loaded); bool have_all_data = (terr_pending <= 0); if (!have_all_data && display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data"); } return have_all_data; #else @@ -533,7 +533,7 @@ bool AP_Arming_Copter::pre_arm_proximity_check(bool display_failure) // return false if proximity sensor unhealthy if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check proximity sensor"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check proximity sensor"); } return false; } @@ -573,27 +573,27 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) //check if accelerometers have calibrated and require reboot if (_ins.accel_cal_requires_reboot()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels calibrated requires reboot"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels calibrated requires reboot"); } return false; } if (!_ins.get_accel_health_all()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accels not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Accels not healthy"); } return false; } if (!_ins.get_gyro_health_all()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy"); } return false; } // get ekf attitude (if bad, it's usually the gyro biases) if (!pre_arm_ekf_attitude_check()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling"); } return false; } @@ -602,7 +602,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) // always check if inertial nav has started and is ready if (!ahrs.healthy()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks"); } return false; } @@ -610,14 +610,14 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) // check compass health if (!_compass.healthy()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass not healthy"); } return false; } if (_compass.is_calibrating()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running"); } return false; } @@ -625,7 +625,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) //check if compass has calibrated and requires reboot if (_compass.compass_cal_requires_reboot()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot"); } return false; } @@ -635,7 +635,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) // always check if the current mode allows arming if (!copter.mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); } return false; } @@ -653,7 +653,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) // if we are using motor interlock switch and it's enabled, fail to arm // skip check in Throw mode which takes control of the motor interlock if (copter.ap.using_interlock && copter.motors->get_interlock()) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled"); return false; } @@ -663,7 +663,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) copter.set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position } else if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && copter.ap.motor_emergency_stop){ - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); return false; } @@ -677,7 +677,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) // baro health check if (!barometer.all_healthy()) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy"); } return false; } @@ -688,7 +688,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref && (fabsf(_inav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity"); } return false; } @@ -709,7 +709,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > copter.aparm.angle_max) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning"); } return false; } @@ -719,7 +719,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { if (copter.failsafe.battery || (!copter.ap.usb_connected && copter.battery.exhausted(copter.g.fs_batt_voltage, copter.g.fs_batt_mah))) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery"); } return false; } @@ -736,7 +736,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { if (copter.failsafe.adsb) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: ADSB threat detected"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: ADSB threat detected"); } return false; } @@ -748,9 +748,9 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe"); #else - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe"); #endif } return false; @@ -762,9 +762,9 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); #else - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); #endif } return false; @@ -773,9 +773,9 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((copter.mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && copter.channel_throttle->get_control_in() > 0) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); #else - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); #endif } return false; @@ -786,7 +786,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch"); } return false; } @@ -814,8 +814,3 @@ void AP_Arming_Copter::set_pre_arm_rc_check(bool b) copter.ap.pre_arm_rc_check = b; } } - -void AP_Arming_Copter::gcs_send_text(MAV_SEVERITY severity, const char *str) -{ - copter.gcs_send_text(severity, str); -} diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index fc1be61871..cf3934cd39 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -49,8 +49,6 @@ protected: private: - void gcs_send_text(MAV_SEVERITY severity, const char *str); - const AP_InertialNav_NavEKF &_inav; const AP_InertialSensor &_ins; const AP_AHRS_NavEKF &_ahrs_navekf; diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 3e902b75a6..9914d9cf48 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -192,7 +192,7 @@ void Copter::perf_update(void) if (should_log(MASK_LOG_PM)) 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/ArduCopter/Copter.h b/ArduCopter/Copter.h index bebe4a9da2..eea8bd25ca 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -722,7 +722,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_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); @@ -1106,7 +1105,6 @@ private: void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); void print_hit_enter(); void tuning(); - 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/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 3a415a66b0..6a95ce0848 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -2038,7 +2038,7 @@ void Copter::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"); } check_usb_mux(); @@ -2078,26 +2078,6 @@ void Copter::gcs_check_input(void) gcs().update(); } -void Copter::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 Copter::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); -} - /* return true if we will accept this packet. Used to implement SYSID_ENFORCE */ diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 55f32c9b7f..0843b8792e 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -150,9 +150,9 @@ int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv) void Copter::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"); } #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 1abe719a4c..cfcf0e3852 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -117,10 +117,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd) #if AC_FENCE == ENABLED if (cmd.p1 == 0) { //disable copter.fence.enable(false); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Fence Disabled"); + gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); } else { //enable fence copter.fence.enable(true); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Fence Enabled"); + gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); } #endif //AC_FENCE == ENABLED break; @@ -273,7 +273,7 @@ bool Copter::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; } @@ -593,7 +593,7 @@ void Copter::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 PARACHUTE == ENABLED @@ -727,7 +727,7 @@ bool Copter::verify_payload_place() case PayloadPlaceStateType_Calibrating_Hover: case PayloadPlaceStateType_Descending_Start: case PayloadPlaceStateType_Descending: - gcs_send_text_fmt(MAV_SEVERITY_INFO, "NAV_PLACE: landed"); + gcs().send_text(MAV_SEVERITY_INFO, "NAV_PLACE: landed"); nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; break; case PayloadPlaceStateType_Releasing_Start: @@ -765,7 +765,7 @@ bool Copter::verify_payload_place() // we have a valid calibration. Hopefully. nav_payload_place.hover_throttle_level = current_throttle_level; const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors->get_throttle_hover()); - gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast(hover_throttle_delta)); + gcs().send_text(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast(hover_throttle_delta)); nav_payload_place.state = PayloadPlaceStateType_Descending_Start; } // no break @@ -781,7 +781,7 @@ bool Copter::verify_payload_place() if (!is_zero(nav_payload_place.descend_max) && nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) { nav_payload_place.state = PayloadPlaceStateType_Ascending; - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent"); + gcs().send_text(MAV_SEVERITY_WARNING, "Reached maximum descent"); return false; // we'll do any cleanups required next time through the loop } // see if we've been descending long enough to calibrate a descend-throttle-level: @@ -812,15 +812,15 @@ bool Copter::verify_payload_place() case PayloadPlaceStateType_Releasing_Start: #if GRIPPER_ENABLED == ENABLED if (g2.gripper.valid()) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper"); + gcs().send_text(MAV_SEVERITY_INFO, "Releasing the gripper"); g2.gripper.release(); } else { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid"); + gcs().send_text(MAV_SEVERITY_INFO, "Gripper not valid"); nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; break; } #else - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper code disabled"); + gcs().send_text(MAV_SEVERITY_INFO, "Gripper code disabled"); #endif nav_payload_place.state = PayloadPlaceStateType_Releasing; // no break @@ -879,7 +879,7 @@ bool Copter::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; @@ -961,7 +961,7 @@ bool Copter::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/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 73ce4e7825..aed24e8e07 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -25,7 +25,7 @@ bool Copter::auto_init(bool ignore_checks) // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); return false; } diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 3afda21c49..6d85c0da5e 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -1230,19 +1230,19 @@ void Copter::autotune_update_gcs(uint8_t message_id) { switch (message_id) { case AUTOTUNE_MESSAGE_STARTED: - gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Started"); + gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Started"); break; case AUTOTUNE_MESSAGE_STOPPED: - gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped"); + gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped"); break; case AUTOTUNE_MESSAGE_SUCCESS: - gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Success"); + gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Success"); break; case AUTOTUNE_MESSAGE_FAILED: - gcs_send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); break; case AUTOTUNE_MESSAGE_SAVED_GAINS: - gcs_send_text(MAV_SEVERITY_INFO,"AutoTune: Saved gains"); + gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Saved gains"); break; } } diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index e5329543c3..33958a76f4 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -29,7 +29,7 @@ void Copter::rtl_restart_without_terrain() if (rtl_path.terrain_used) { rtl_build_path(false); rtl_climb_start(); - gcs_send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); } } diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp index f9cf4da02d..303e3a57b3 100644 --- a/ArduCopter/control_throw.cpp +++ b/ArduCopter/control_throw.cpp @@ -39,18 +39,18 @@ void Copter::throw_run() throw_state.stage = Throw_Disarmed; } else if (throw_state.stage == Throw_Disarmed && motors->armed()) { - gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw"); + gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw"); throw_state.stage = Throw_Detecting; } else if (throw_state.stage == Throw_Detecting && throw_detected()){ - gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting"); + gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting"); throw_state.stage = Throw_Uprighting; // Cancel the waiting for throw tone sequence AP_Notify::flags.waiting_for_throw = false; } else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) { - gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height"); + gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height"); throw_state.stage = Throw_HgtStabilise; // initialize vertical speed and acceleration limits @@ -74,7 +74,7 @@ void Copter::throw_run() set_auto_armed(true); } else if (throw_state.stage == Throw_HgtStabilise && throw_height_good()) { - gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position"); + gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position"); throw_state.stage = Throw_PosHold; // initialise the loiter target to the curent position and velocity diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index e8a1c4fab0..0026102fa1 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -45,7 +45,7 @@ void Copter::crash_check() // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); // send message to gcs - gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming"); + gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming"); // disarm motors init_disarm_motors(); } @@ -136,7 +136,7 @@ void Copter::parachute_check() void Copter::parachute_release() { // send message to gcs and dataflash - gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released"); + gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Released"); Log_Write_Event(DATA_PARACHUTE_RELEASED); // disarm motors @@ -162,7 +162,7 @@ void Copter::parachute_manual_release() // do not release if we are landed or below the minimum altitude above home if (ap.land_complete) { // warn user of reason for failure - gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Landed"); + gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Landed"); // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED); return; @@ -171,7 +171,7 @@ void Copter::parachute_manual_release() // do not release if we are landed or below the minimum altitude above home if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) { // warn user of reason for failure - gcs_send_text(MAV_SEVERITY_ALERT,"Parachute: Too low"); + gcs().send_text(MAV_SEVERITY_ALERT,"Parachute: Too low"); // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW); return; diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 70491b38c7..2ac92ce545 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -58,7 +58,7 @@ void Copter::ekf_check() Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE); // send message to gcs if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); ekf_check_state.last_warn_time = AP_HAL::millis(); } failsafe_ekf_event(); @@ -192,7 +192,7 @@ void Copter::check_ekf_reset() if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) { ekf_primary_core = EKF2.getPrimaryCoreIndex(); Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core); - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "EKF primary changed:%d\n", (unsigned)ekf_primary_core); + gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d\n", (unsigned)ekf_primary_core); } #endif } diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 64283c2cb4..07bb7be785 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -44,7 +44,7 @@ void Copter::esc_calibration_startup_check() // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); // send message to gcs - gcs_send_text(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board"); // turn on esc calibration notification AP_Notify::flags.esc_calibration = true; // block until we restart @@ -95,7 +95,7 @@ void Copter::esc_calibration_passthrough() } // send message to GCS - gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs"); + gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs"); // disable safety if requested BoardConfig.init_safety(); @@ -143,7 +143,7 @@ void Copter::esc_calibration_auto() } // send message to GCS - gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Auto calibration"); + gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Auto calibration"); // disable safety if requested BoardConfig.init_safety(); @@ -163,7 +163,7 @@ void Copter::esc_calibration_auto() // wait for safety switch to be pressed while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (!printed_msg) { - gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch"); + gcs().send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch"); printed_msg = true; } hal.rcout->cork(); diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 36b78cd748..185980a18a 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -66,7 +66,7 @@ void Copter::failsafe_battery_event(void) set_failsafe_battery(true); // warn the ground station and log to dataflash - gcs_send_text(MAV_SEVERITY_WARNING,"Low battery"); + gcs().send_text(MAV_SEVERITY_WARNING,"Low battery"); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); } @@ -172,7 +172,7 @@ void Copter::failsafe_terrain_set_status(bool data_ok) void Copter::failsafe_terrain_on_event() { failsafe.terrain = true; - gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); + gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED); if (should_disarm_on_failsafe()) { diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 84977d417e..f0040daa0b 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -142,7 +142,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) } else { // Log error that we failed to enter desired flight mode Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); - gcs_send_text(MAV_SEVERITY_WARNING,"Flight mode change failed"); + gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed"); } // update notify object diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 6eb991cae5..ad116bd65f 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -160,7 +160,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs_send_text(MAV_SEVERITY_INFO, "Arming motors"); + gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); #endif // Remember Orientation @@ -228,7 +228,7 @@ void Copter::init_disarm_motors() } #if HIL_MODE != HIL_MODE_DISABLED || 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 @@ -331,7 +331,7 @@ void Copter::lost_vehicle_check() if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (AP_Notify::flags.vehicle_lost == false) { AP_Notify::flags.vehicle_lost = true; - gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm"); + gcs().send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm"); } } else { soundalarm_counter++; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index f2668dbddc..2fea7d5957 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -2,13 +2,13 @@ void Copter::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"); } // return barometric altitude in centimeters diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 211b6d813c..b7f9e92828 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -605,7 +605,7 @@ void Copter::save_trim() float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); ahrs.add_trim(roll_trim, pitch_trim); Log_Write_Event(DATA_SAVE_TRIM); - gcs_send_text(MAV_SEVERITY_INFO, "Trim saved"); + gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); } // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index b8f809c2b5..812c274f07 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -262,7 +262,7 @@ void Copter::init_ardupilot() 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"); delay(1000); }