From 7377b3f8f2336c43d9fbdf254c0df8612f46af87 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 14 Jul 2023 10:58:09 +1000 Subject: [PATCH] ArduCopter: allow compilation with HAL_LOGGING_ENABLED false --- ArduCopter/AP_Arming.cpp | 8 +++++++- ArduCopter/AP_State.cpp | 8 ++++---- ArduCopter/Copter.cpp | 23 ++++++++++++++++++++--- ArduCopter/Copter.h | 4 ++++ ArduCopter/GCS_Mavlink.cpp | 4 ++++ ArduCopter/Log.cpp | 27 ++------------------------- ArduCopter/Parameters.cpp | 2 ++ ArduCopter/RC_Channel.cpp | 16 ++++++++-------- ArduCopter/avoidance_adsb.cpp | 6 ++++-- ArduCopter/config.h | 5 +---- ArduCopter/crash_check.cpp | 10 +++++----- ArduCopter/ekf_check.cpp | 16 ++++++++-------- ArduCopter/events.cpp | 20 ++++++++++---------- ArduCopter/failsafe.cpp | 4 ++-- ArduCopter/fence.cpp | 4 ++-- ArduCopter/heli.cpp | 4 ++-- ArduCopter/land_detector.cpp | 4 +++- ArduCopter/mode.cpp | 12 +++++++----- ArduCopter/mode.h | 2 ++ ArduCopter/mode_auto.cpp | 14 ++++++++++---- ArduCopter/mode_autotune.cpp | 2 ++ ArduCopter/mode_flip.cpp | 6 +++--- ArduCopter/mode_flowhold.cpp | 5 +++++ ArduCopter/mode_guided.cpp | 25 +++++++++++++++++++++---- ArduCopter/mode_land.cpp | 2 +- ArduCopter/mode_rtl.cpp | 16 +++++++++------- ArduCopter/mode_systemid.cpp | 2 ++ ArduCopter/mode_throw.cpp | 6 ++++-- ArduCopter/mode_zigzag.cpp | 4 ++-- ArduCopter/motors.cpp | 4 ++-- ArduCopter/radio.cpp | 2 +- ArduCopter/system.cpp | 10 +++++----- ArduCopter/terrain.cpp | 2 ++ ArduCopter/toy_mode.cpp | 3 ++- ArduCopter/tuning.cpp | 2 ++ 35 files changed, 170 insertions(+), 114 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 72bc9addf0..30226a9020 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -694,8 +694,10 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ return false; } +#if HAL_LOGGING_ENABLED // let logger know that we're armed (it may open logs e.g.) AP::logger().set_vehicle_armed(true); +#endif // disable cpu failsafe because initialising everything takes a while copter.failsafe_disable(); @@ -722,7 +724,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ if (!ahrs.home_is_set()) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.resetHeightDatum(); - AP::logger().Write_Event(LogEvent::EKF_ALT_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_ALT_RESET); // we have reset height, so arming height is zero copter.arming_altitude_m = 0; @@ -755,8 +757,10 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ // finally actually arm the motors copter.motors->armed(true); +#if HAL_LOGGING_ENABLED // log flight mode in case it was changed while vehicle was disarmed AP::logger().Write_Mode((uint8_t)copter.flightmode->mode_number(), copter.control_mode_reason); +#endif // re-enable failsafe copter.failsafe_enable(); @@ -837,7 +841,9 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che copter.mode_auto.mission.reset(); #endif +#if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(false); +#endif hal.util->set_soft_armed(false); diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index a3e60e8076..3fe709b83e 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -9,7 +9,7 @@ void Copter::set_auto_armed(bool b) ap.auto_armed = b; if(b){ - AP::logger().Write_Event(LogEvent::AUTO_ARMED); + LOGGER_WRITE_EVENT(LogEvent::AUTO_ARMED); } } @@ -24,17 +24,17 @@ void Copter::set_simple_mode(SimpleMode b) if (simple_mode != b) { switch (b) { case SimpleMode::NONE: - AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF); + LOGGER_WRITE_EVENT(LogEvent::SET_SIMPLE_OFF); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off"); break; case SimpleMode::SIMPLE: - AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON); + LOGGER_WRITE_EVENT(LogEvent::SET_SIMPLE_ON); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on"); break; case SimpleMode::SUPERSIMPLE: // initialise super simple heading update_super_simple_bearing(true); - AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON); + LOGGER_WRITE_EVENT(LogEvent::SET_SUPERSIMPLE_ON); gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on"); break; } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index c121865134..740bb95e5f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -143,7 +143,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { // camera mount's fast update FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast), #endif +#if HAL_LOGGING_ENABLED FAST_TASK(Log_Video_Stabilisation), +#endif SCHED_TASK(rc_loop, 250, 130, 3), SCHED_TASK(throttle_loop, 50, 75, 6), @@ -188,7 +190,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if FRAME_CONFIG == HELI_FRAME SCHED_TASK(check_dynamic_flight, 50, 75, 72), #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75), #endif SCHED_TASK(one_hz_loop, 1, 100, 81), @@ -209,14 +211,16 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if AP_CAMERA_ENABLED SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111), #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK(ten_hz_logging_loop, 10, 350, 114), SCHED_TASK(twentyfive_hz_logging, 25, 110, 117), SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300, 120), #endif SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123), +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126), +#endif #if AP_RPM_ENABLED SCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129), #endif @@ -512,6 +516,7 @@ void Copter::update_batt_compass(void) } } +#if HAL_LOGGING_ENABLED // Full rate logging of attitude, rate and pid loops // should be run at loop rate void Copter::loop_rate_logging() @@ -609,6 +614,7 @@ void Copter::twentyfive_hz_logging() } #endif } +#endif // HAL_LOGGING_ENABLED // three_hz_loop - 3hz loop void Copter::three_hz_loop() @@ -638,9 +644,11 @@ void Copter::three_hz_loop() // one_hz_loop - runs at 1Hz void Copter::one_hz_loop() { +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::AP_STATE, ap.value); } +#endif if (!motors->armed()) { update_using_interlock(); @@ -657,8 +665,10 @@ void Copter::one_hz_loop() // update assigned functions and enable auxiliary servos SRV_Channels::enable_aux_servos(); +#if HAL_LOGGING_ENABLED // log terrain data terrain_logging(); +#endif #if HAL_ADSB_ENABLED adsb.set_is_flying(!ap.land_complete); @@ -685,10 +695,12 @@ void Copter::init_simple_bearing() super_simple_cos_yaw = simple_cos_yaw; super_simple_sin_yaw = simple_sin_yaw; +#if HAL_LOGGING_ENABLED // log the simple bearing if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor); } +#endif } // update_simple_mode - rotates pilot input if we are in simple mode @@ -757,6 +769,7 @@ void Copter::update_altitude() // read in baro altitude read_barometer(); +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); if (!should_log(MASK_LOG_FTN_FAST)) { @@ -766,6 +779,7 @@ void Copter::update_altitude() #endif } } +#endif } // vehicle specific waypoint info helpers @@ -808,7 +822,10 @@ bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const constructor for main Copter class */ Copter::Copter(void) - : logger(g.log_bitmask), + : +#if HAL_LOGGING_ENABLED + logger(g.log_bitmask), +#endif flight_modes(&g.flight_mode1), simple_cos_yaw(1.0f), super_simple_cos_yaw(1.0), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bb468a3993..8685f4cb8f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -252,7 +252,9 @@ private: RC_Channel *channel_throttle; RC_Channel *channel_yaw; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif // flight modes convenience array AP_Int8 *flight_modes; @@ -836,6 +838,7 @@ private: // standby.cpp void standby_update(); +#if HAL_LOGGING_ENABLED // Log.cpp void Log_Write_Control_Tuning(); void Log_Write_Attitude(); @@ -857,6 +860,7 @@ private: void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); void log_init(void); +#endif // HAL_LOGGING_ENABLED // mode.cpp bool set_mode(Mode::Number mode, ModeReason reason); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 7f86eabc8e..e07bd005d0 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1488,7 +1488,11 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109 { +#if HAL_LOGGING_ENABLED handle_radio_status(msg, copter.should_log(MASK_LOG_PM)); +#else + handle_radio_status(msg, false); +#endif break; } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 027544771d..4d9e4ef87b 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Code to Write and Read packets from AP_Logger log memory // Code to interact with the user to dump or erase logs @@ -576,27 +576,4 @@ void Copter::log_init(void) logger.Init(log_structure, ARRAY_SIZE(log_structure)); } -#else // LOGGING_ENABLED - -void Copter::Log_Write_Control_Tuning() {} -void Copter::Log_Write_Attitude(void) {} -void Copter::Log_Write_EKF_POS() {} -void Copter::Log_Write_Data(LogDataID id, int32_t value) {} -void Copter::Log_Write_Data(LogDataID id, uint32_t value) {} -void Copter::Log_Write_Data(LogDataID id, int16_t value) {} -void Copter::Log_Write_Data(LogDataID id, uint16_t value) {} -void Copter::Log_Write_Data(LogDataID id, float value) {} -void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} -void Copter::Log_Write_Guided_Position_Target(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) {} -void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate) {} -void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} -void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} -void Copter::Log_Write_Vehicle_Startup_Messages() {} - -#if FRAME_CONFIG == HELI_FRAME -void Copter::Log_Write_Heli() {} -#endif - -void Copter::log_init(void) {} - -#endif // LOGGING_ENABLED +#endif // HAL_LOGGING_ENABLED diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index e3b949b7c3..d1de6fe45d 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -554,9 +554,11 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(camera_mount, "MNT", AP_Mount), #endif +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index f400694b45..b613cb141a 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -229,7 +229,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi // only altitude will matter to the AP mission script for takeoff. if (copter.mode_auto.mission.add_cmd(cmd)) { // log event - AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); + LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP); } } @@ -247,7 +247,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi // save command if (copter.mode_auto.mission.add_cmd(cmd)) { // log event - AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); + LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP); } } #endif @@ -276,15 +276,15 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi switch(ch_flag) { case AuxSwitchPos::LOW: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::OFF); - AP::logger().Write_Event(LogEvent::ACRO_TRAINER_OFF); + LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_OFF); break; case AuxSwitchPos::MIDDLE: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LEVELING); - AP::logger().Write_Event(LogEvent::ACRO_TRAINER_LEVELING); + LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LEVELING); break; case AuxSwitchPos::HIGH: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LIMITED); - AP::logger().Write_Event(LogEvent::ACRO_TRAINER_LIMITED); + LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LIMITED); break; } #endif @@ -540,12 +540,12 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi switch (ch_flag) { case AuxSwitchPos::HIGH: copter.standby_active = true; - AP::logger().Write_Event(LogEvent::STANDBY_ENABLE); + LOGGER_WRITE_EVENT(LogEvent::STANDBY_ENABLE); gcs().send_text(MAV_SEVERITY_INFO, "Stand By Enabled"); break; default: copter.standby_active = false; - AP::logger().Write_Event(LogEvent::STANDBY_DISABLE); + LOGGER_WRITE_EVENT(LogEvent::STANDBY_DISABLE); gcs().send_text(MAV_SEVERITY_INFO, "Stand By Disabled"); break; } @@ -683,7 +683,7 @@ void Copter::save_trim() float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f); float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); ahrs.add_trim(roll_trim, pitch_trim); - AP::logger().Write_Event(LogEvent::SAVE_TRIM); + LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM); gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); } diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index 85fee2d752..bd541c3551 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -88,10 +88,12 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O } } +#if HAL_LOGGING_ENABLED if (failsafe_state_change) { AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_ADSB, LogErrorCode(actual_action)); } +#endif // return with action taken return actual_action; @@ -102,8 +104,8 @@ void AP_Avoidance_Copter::handle_recovery(RecoveryAction recovery_action) // check we are coming out of failsafe if (copter.failsafe.adsb) { copter.failsafe.adsb = false; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_ADSB, - LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_ADSB, + LogErrorCode::ERROR_RESOLVED); // restore flight mode if requested and user has not changed mode since if (copter.control_mode_reason == ModeReason::AVOIDANCE) { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index ace8cc43f2..97793741c9 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -239,7 +239,7 @@ ////////////////////////////////////////////////////////////////////////////// // System ID - conduct system identification tests on vehicle #ifndef MODE_SYSTEMID_ENABLED -# define MODE_SYSTEMID_ENABLED ENABLED +# define MODE_SYSTEMID_ENABLED HAL_LOGGING_ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -529,9 +529,6 @@ ////////////////////////////////////////////////////////////////////////////// // Logging control // -#ifndef LOGGING_ENABLED - # define LOGGING_ENABLED ENABLED -#endif // Default logging bitmask #ifndef DEFAULT_LOG_BITMASK diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 786e56b5a0..2ce5d89420 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -88,7 +88,7 @@ void Copter::crash_check() // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) { - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming: AngErr=%.0f>%.0f, Accel=%.1f<%.1f", angle_error, CRASH_CHECK_ANGLE_DEVIATION_DEG, filtered_acc, CRASH_CHECK_ACCEL_MAX); // disarm motors @@ -163,7 +163,7 @@ void Copter::thrust_loss_check() if (thrust_loss_counter >= (THRUST_LOSS_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) { // reset counter thrust_loss_counter = 0; - AP::logger().Write_Error(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED); // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor() + 1); // enable thrust loss handling @@ -322,7 +322,7 @@ void Copter::parachute_check() } else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) { // reset control loss counter control_loss_count = 0; - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL); // release parachute parachute_release(); } @@ -357,7 +357,7 @@ void Copter::parachute_manual_release() if (ap.land_complete) { // warn user of reason for failure gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Landed"); - AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_LANDED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_LANDED); return; } @@ -365,7 +365,7 @@ void Copter::parachute_manual_release() 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"); - AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TOO_LOW); + LOGGER_WRITE_ERROR(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TOO_LOW); return; } diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 12a582d71c..87cbd20f6e 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -80,7 +80,7 @@ void Copter::ekf_check() // limit count from climbing too high ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_variance = true; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::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"); @@ -97,7 +97,7 @@ void Copter::ekf_check() // if variances are flagged as bad and the counter reaches zero then clear flag if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { ekf_check_state.bad_variance = false; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -156,7 +156,7 @@ void Copter::failsafe_ekf_event() { // EKF failsafe event has occurred failsafe.ekf = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // if disarmed take no action if (!motors->armed()) { @@ -207,7 +207,7 @@ void Copter::failsafe_ekf_off_event(void) AP_Notify::flags.failsafe_ekf = false; gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe Cleared"); } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); } // re-check if the flight mode requires GPS but EKF failsafe is active @@ -232,14 +232,14 @@ void Copter::check_ekf_reset() if (new_ekfYawReset_ms != ekfYawReset_ms) { attitude_control->inertial_frame_reset(); ekfYawReset_ms = new_ekfYawReset_ms; - AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET); } // check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) { attitude_control->inertial_frame_reset(); ekf_primary_core = ahrs.get_primary_core_index(); - AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); } } @@ -285,7 +285,7 @@ void Copter::check_vibration() vibration_check.clear_ms = 0; vibration_check.high_vibes = true; pos_control->set_vibe_comp(true); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); } } else { @@ -300,7 +300,7 @@ void Copter::check_vibration() vibration_check.high_vibes = false; pos_control->set_vibe_comp(false); vibration_check.clear_ms = 0; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); } } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 94077c5088..2160e22249 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -12,7 +12,7 @@ bool Copter::failsafe_option(FailsafeOption opt) const void Copter::failsafe_radio_on_event() { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); // set desired action based on FS_THR_ENABLE parameter FailsafeAction desired_action; @@ -83,7 +83,7 @@ void Copter::failsafe_radio_off_event() { // no need to do anything except log the error as resolved // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared"); } @@ -98,7 +98,7 @@ void Copter::announce_failsafe(const char *type, const char *action_undertaken) void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); FailsafeAction desired_action = (FailsafeAction)action; @@ -162,7 +162,7 @@ void Copter::failsafe_gcs_check() // failsafe_gcs_on_event - actions to take when GCS contact is lost void Copter::failsafe_gcs_on_event(void) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); RC_Channels::clear_overrides(); // convert the desired failsafe response to the FailsafeAction enum @@ -236,7 +236,7 @@ void Copter::failsafe_gcs_on_event(void) void Copter::failsafe_gcs_off_event(void) { gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared"); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); } // executes terrain failsafe if data is missing for longer than a few seconds @@ -251,7 +251,7 @@ void Copter::failsafe_terrain_check() if (trigger_event) { failsafe_terrain_on_event(); } else { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); failsafe.terrain = false; } } @@ -282,7 +282,7 @@ void Copter::failsafe_terrain_on_event() { failsafe.terrain = true; gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); if (should_disarm_on_failsafe()) { arming.disarm(AP_Arming::Method::TERRAINFAILSAFE); @@ -306,10 +306,10 @@ void Copter::gpsglitch_check() if (ap.gps_glitching != gps_glitching) { ap.gps_glitching = gps_glitching; if (gps_glitching) { - AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch or Compass error"); } else { - AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL,"Glitch cleared"); } } @@ -361,7 +361,7 @@ void Copter::failsafe_deadreckon_check() if (failsafe.deadreckon && copter.flightmode->requires_GPS()) { // log error - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED); // immediately disarm while landed if (should_disarm_on_failsafe()) { diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 9adf95db03..88ae35572f 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -43,7 +43,7 @@ void Copter::failsafe_check() failsafe_last_timestamp = tnow; if (in_failsafe) { in_failsafe = false; - AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); } return; } @@ -58,7 +58,7 @@ void Copter::failsafe_check() motors->output_min(); } - AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); } if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 122d49a965..3bfe085c2b 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -79,11 +79,11 @@ void Copter::fence_check() } } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches) { // record clearing of breach - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } } diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index fb7bb9babf..a9c9fde1ac 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -184,9 +184,9 @@ void Copter::heli_update_rotor_speed_targets() // when rotor_runup_complete changes to true, log event if (!rotor_runup_complete_last && motors->rotor_runup_complete()) { - AP::logger().Write_Event(LogEvent::ROTOR_RUNUP_COMPLETE); + LOGGER_WRITE_EVENT(LogEvent::ROTOR_RUNUP_COMPLETE); } else if (rotor_runup_complete_last && !motors->rotor_runup_complete() && !heli_flags.in_autorotation) { - AP::logger().Write_Event(LogEvent::ROTOR_SPEED_BELOW_CRITICAL); + LOGGER_WRITE_EVENT(LogEvent::ROTOR_SPEED_BELOW_CRITICAL); } rotor_runup_complete_last = motors->rotor_runup_complete(); } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index c6b038c4a7..4a4cdd3fca 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -139,11 +139,13 @@ void Copter::set_land_complete(bool b) land_detector_count = 0; +#if HAL_LOGGING_ENABLED if(b){ AP::logger().Write_Event(LogEvent::LAND_COMPLETE); } else { AP::logger().Write_Event(LogEvent::NOT_LANDED); } +#endif ap.land_complete = b; #if STATS_ENABLED == ENABLED @@ -170,7 +172,7 @@ void Copter::set_land_complete_maybe(bool b) return; if (b) { - AP::logger().Write_Event(LogEvent::LAND_COMPLETE_MAYBE); + LOGGER_WRITE_EVENT(LogEvent::LAND_COMPLETE_MAYBE); } ap.land_complete_maybe = b; } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 3313ab3550..f235cc22ef 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -191,7 +191,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) void Copter::mode_change_failed(const Mode *mode, const char *reason) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number())); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number())); // make sad noise if (copter.ap.initialised) { AP_Notify::events.user_mode_change_failed = 1; @@ -360,7 +360,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) // update flight mode flightmode = new_flightmode; control_mode_reason = reason; +#if HAL_LOGGING_ENABLED logger.Write_Mode((uint8_t)flightmode->mode_number(), reason); +#endif gcs().send_message(MSG_HEARTBEAT); #if HAL_ADSB_ENABLED @@ -700,7 +702,7 @@ void Mode::land_run_horizontal_control() // process pilot inputs if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); @@ -720,7 +722,7 @@ void Mode::land_run_horizontal_control() // record if pilot has overridden roll or pitch if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { - AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); + LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; #if AC_PRECLAND_ENABLED @@ -815,7 +817,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos) { if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); @@ -833,7 +835,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos) // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { if (!copter.ap.land_repo_active) { - AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); + LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE); } // this flag will be checked by prec land state machine later and any further landing retires will be cancelled copter.ap.land_repo_active = true; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index afb348c1dc..dd06ee0149 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -743,7 +743,9 @@ protected: float get_pilot_desired_climb_rate_cms(void) const override; void get_pilot_desired_rp_yrate_cd(float &roll_cd, float &pitch_cd, float &yaw_rate_cds) override; void init_z_limits() override; +#if HAL_LOGGING_ENABLED void log_pids() override; +#endif }; class ModeAutoTune : public Mode { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c33c9d98d7..4185f35e03 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -167,7 +167,9 @@ void ModeAuto::run() if (auto_RTL && (!(mission.get_in_landing_sequence_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE))) { auto_RTL = false; // log exit from Auto RTL +#if HAL_LOGGING_ENABLED copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), ModeReason::AUTO_RTL_EXIT); +#endif } } @@ -219,8 +221,10 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) // if not already in auto switch to auto if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) { auto_RTL = true; +#if HAL_LOGGING_ENABLED // log entry into AUTO RTL copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason); +#endif // make happy noise if (copter.ap.initialised) { @@ -236,7 +240,7 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found"); } - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); // make sad noise if (copter.ap.initialised) { AP_Notify::events.user_mode_change_failed = 1; @@ -338,7 +342,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) // get altitude target above EKF origin if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) { // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); // fall back to altitude above current altitude alt_target_cm = current_alt_cm + dest.alt; } @@ -618,10 +622,12 @@ bool ModeAuto::set_speed_down(float speed_down_cms) // start_command - this function will be called when the ap_mission lib wishes to start a new command bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { +#if HAL_LOGGING_ENABLED // To-Do: logging when new commands start/end if (copter.should_log(MASK_LOG_CMD)) { copter.logger.Write_Mission_Cmd(mission, cmd); } +#endif switch(cmd.id) { @@ -1577,7 +1583,7 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) // this can only fail due to missing terrain database alt or rangefinder alt // use current alt-above-home and report error target_loc.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "Land: no terrain data, using alt-above-home"); } @@ -1954,7 +1960,7 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) // this can only fail due to missing terrain database alt or rangefinder alt // use current alt-above-home and report error target_loc.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "PayloadPlace: no terrain data, using alt-above-home"); } if (!wp_start(target_loc)) { diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 73699663a3..0a7da76b07 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -98,12 +98,14 @@ void AutoTune::init_z_limits() copter.pos_control->set_correction_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z); } +#if HAL_LOGGING_ENABLED void AutoTune::log_pids() { copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info()); copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info()); copter.logger.Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info()); } +#endif /* check if we have a good position estimate diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index bade71e5e9..938b44db7d 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -76,7 +76,7 @@ bool ModeFlip::init(bool ignore_checks) } // log start of flip - AP::logger().Write_Event(LogEvent::FLIP_START); + LOGGER_WRITE_EVENT(LogEvent::FLIP_START); // capture current attitude which will be used during the FlipState::Recovery stage const float angle_max = copter.aparm.angle_max; @@ -194,7 +194,7 @@ void ModeFlip::run() copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN); } // log successful completion - AP::logger().Write_Event(LogEvent::FLIP_END); + LOGGER_WRITE_EVENT(LogEvent::FLIP_END); } break; @@ -206,7 +206,7 @@ void ModeFlip::run() copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN); } // log abandoning flip - AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); break; } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 91814ab45f..ddb55d3d6d 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -202,6 +202,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) bf_angles.x = constrain_float(bf_angles.x, -copter.aparm.angle_max, copter.aparm.angle_max); bf_angles.y = constrain_float(bf_angles.y, -copter.aparm.angle_max, copter.aparm.angle_max); +#if HAL_LOGGING_ENABLED // @LoggerMessage: FHLD // @Description: FlowHold mode messages // @URL: https://ardupilot.org/copter/docs/flowhold-mode.html @@ -222,6 +223,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) (double)quality_filtered, (double)xy_I.x, (double)xy_I.y); } +#endif // HAL_LOGGING_ENABLED } // flowhold_run - runs the flowhold controller @@ -478,6 +480,7 @@ void ModeFlowHold::update_height_estimate(void) // new height estimate for logging height_estimate = ins_height + height_offset; +#if HAL_LOGGING_ENABLED // @LoggerMessage: FHXY // @Description: Height estimation using optical flow sensor // @Field: TimeUS: Time since system startup @@ -504,6 +507,8 @@ void ModeFlowHold::update_height_estimate(void) (double)ins_height, (double)last_ins_height, dt_ms); +#endif + gcs().send_named_float("HEST", height_estimate); delta_velocity_ne.zero(); last_ins_height = ins_height; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 9c2914de8b..ba9a52f60e 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -332,7 +332,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // reject destination if outside the fence const Location dest_loc(destination, terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -351,8 +351,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // no need to check return status because terrain data is not used wp_nav->set_wp_destination(destination, terrain_alt); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, destination, terrain_alt, Vector3f(), Vector3f()); +#endif send_notification = true; return true; } @@ -391,8 +393,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa guided_accel_target_cmss.zero(); update_time_ms = millis(); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); +#endif send_notification = true; @@ -424,7 +428,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails if (!copter.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -438,7 +442,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y if (!wp_nav->set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } @@ -446,8 +450,11 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // set yaw state set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f()); +#endif + send_notification = true; return true; } @@ -493,7 +500,9 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y update_time_ms = millis(); // log target +#if HAL_LOGGING_ENABLED copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); +#endif send_notification = true; @@ -518,10 +527,12 @@ void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw guided_accel_target_cmss = acceleration; update_time_ms = millis(); +#if HAL_LOGGING_ENABLED // log target if (log_request) { copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } +#endif } // set_velocity - sets guided mode's target velocity @@ -548,10 +559,12 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera guided_accel_target_cmss = acceleration; update_time_ms = millis(); +#if HAL_LOGGING_ENABLED // log target if (log_request) { copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } +#endif } // set_destination_posvel - set guided mode position and velocity target @@ -567,7 +580,7 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -587,8 +600,10 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const guided_vel_target_cms = velocity; guided_accel_target_cmss = acceleration; +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); +#endif return true; } @@ -648,8 +663,10 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ float roll_rad, pitch_rad, yaw_rad; attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01); +#endif } // takeoff_run - takeoff in guided mode diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 377bef5ee9..1f34e20fbf 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -102,7 +102,7 @@ void ModeLand::nogps_run() // process pilot inputs if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index a8421d9009..b8a6cd9bbe 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -39,7 +39,9 @@ bool ModeRTL::init(bool ignore_checks) // re-start RTL with terrain following disabled void ModeRTL::restart_without_terrain() { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); +#if HAL_LOGGING_ENABLED + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); +#endif terrain_following_allowed = false; _state = SubMode::STARTING; _state_complete = true; @@ -134,7 +136,7 @@ void ModeRTL::climb_start() if (!wp_nav->set_wp_destination_loc(rtl_path.climb_target) || !wp_nav->set_wp_destination_next_loc(rtl_path.return_target)) { // this should not happen because rtl_build_path will have checked terrain data was available gcs().send_text(MAV_SEVERITY_CRITICAL,"RTL: unexpected error setting climb target"); - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE); return; } @@ -275,7 +277,7 @@ void ModeRTL::descent_run() // process pilot's input if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!copter.set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); @@ -292,7 +294,7 @@ void ModeRTL::descent_run() // record if pilot has overridden roll or pitch if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { - AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); + LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; } @@ -426,7 +428,7 @@ void ModeRTL::compute_return_target() switch (wp_nav->get_terrain_source()) { case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE: alt_type = ReturnTargetAltType::RELATIVE; - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home"); break; case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER: @@ -447,7 +449,7 @@ void ModeRTL::compute_return_target() // fallback to relative alt and warn user alt_type = ReturnTargetAltType::RELATIVE; gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: rangefinder unhealthy, using alt-above-home"); - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); } } @@ -463,7 +465,7 @@ void ModeRTL::compute_return_target() } else { // fallback to relative alt and warn user alt_type = ReturnTargetAltType::RELATIVE; - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home"); } } diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 15dc1eba4f..801f903205 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -99,7 +99,9 @@ bool ModeSystemId::init(bool ignore_checks) gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis); +#if HAL_LOGGING_ENABLED copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); +#endif return true; } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index a2f71ff42f..884b7a22be 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -200,6 +200,7 @@ void ModeThrow::run() break; } +#if HAL_LOGGING_ENABLED // log at 10hz or if stage changes uint32_t now = AP_HAL::millis(); if ((stage != prev_stage) || (now - last_log_ms) > 100) { @@ -213,7 +214,7 @@ void ModeThrow::run() const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good(); const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good(); const bool pos_ok = (stage > Throw_PosHold) || throw_position_good(); - + // @LoggerMessage: THRO // @Description: Throw Mode messages // @URL: https://ardupilot.org/copter/docs/throw-mode.html @@ -227,7 +228,7 @@ void ModeThrow::run() // @Field: AttOk: True if the vehicle is upright // @Field: HgtOk: True if the vehicle is within 50cm of the demanded height // @Field: PosOk: True if the vehicle is within 50cm of the demanded horizontal position - + AP::logger().WriteStreaming( "THRO", "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk", @@ -245,6 +246,7 @@ void ModeThrow::run() height_ok, pos_ok); } +#endif // HAL_LOGGING_ENABLED } bool ModeThrow::throw_detected() diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 2952158ec6..fbf43aa50e 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -171,12 +171,12 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) // store point A dest_A = curr_pos; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point A stored"); - AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_A); + LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_A); } else { // store point B dest_B = curr_pos; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point B stored"); - AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_B); + LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_B); } // if both A and B have been stored advance state if (!dest_A.is_zero() && !dest_B.is_zero() && !is_zero((dest_B - dest_A).length_squared())) { diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 84ab927339..34a3ff201b 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -165,10 +165,10 @@ void Copter::motors_output() bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); if (!motors->get_interlock() && interlock) { motors->set_interlock(true); - AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED); + LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_ENABLED); } else if (motors->get_interlock() && !interlock) { motors->set_interlock(false); - AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED); + LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_DISABLED); } if (ap.motor_test) { diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 57043fba91..430bed7e93 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -121,7 +121,7 @@ void Copter::read_radio() } // Log an error and enter failsafe. - AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); + LOGGER_WRITE_ERROR(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); set_failsafe_radio(true); } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 40c077fc62..83c4cbf5b8 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -55,7 +55,7 @@ void Copter::init_ardupilot() osd.init(); #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif @@ -183,8 +183,10 @@ void Copter::init_ardupilot() g2.smart_rtl.init(); #endif +#if HAL_LOGGING_ENABLED // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); +#endif startup_INS_ground(); @@ -352,18 +354,16 @@ void Copter::update_auto_armed() } } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ bool Copter::should_log(uint32_t mask) { -#if LOGGING_ENABLED == ENABLED ap.logging_started = logger.logging_started(); return logger.should_log(mask); -#else - return false; -#endif } +#endif /* allocate the motors class diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp index 77765bb61b..5933de460e 100644 --- a/ArduCopter/terrain.cpp +++ b/ArduCopter/terrain.cpp @@ -17,6 +17,7 @@ void Copter::terrain_update() #endif } +#if HAL_LOGGING_ENABLED // log terrain data - should be called at 1hz void Copter::terrain_logging() { @@ -26,3 +27,4 @@ void Copter::terrain_logging() } #endif } +#endif diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index d212cb4335..c1cea4d4cc 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -991,6 +991,7 @@ void ToyMode::thrust_limiting(float *thrust, uint8_t num_motors) uint16_t pwm[4]; hal.rcout->read(pwm, 4); +#if HAL_LOGGING_ENABLED // @LoggerMessage: THST // @Description: Maximum thrust limitation based on battery voltage in Toy Mode // @Field: TimeUS: Time since system startup @@ -1008,7 +1009,7 @@ void ToyMode::thrust_limiting(float *thrust, uint8_t num_motors) (double)thrust_mul, pwm[0], pwm[1], pwm[2], pwm[3]); } - +#endif } #if ENABLE_LOAD_TEST diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index db7f15c889..babc53b9ff 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -28,7 +28,9 @@ void Copter::tuning() const uint16_t radio_in = rc6->get_radio_in(); float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max()); +#if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max); +#endif switch(g.radio_tuning) {