From 2a34de75148cb3bc32c56549785fcd0f47e0bb20 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 23 May 2017 15:00:57 +0930 Subject: [PATCH] Copter: attitude logging at 400hz --- ArduCopter/ArduCopter.cpp | 27 ++++++++++++--------------- ArduCopter/Copter.h | 2 ++ ArduCopter/Log.cpp | 12 ++++++++++++ 3 files changed, 26 insertions(+), 15 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index a71262e856..dde68ff3fc 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -109,6 +109,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if FRAME_CONFIG == HELI_FRAME SCHED_TASK(check_dynamic_flight, 50, 75), #endif + SCHED_TASK(fourhundred_hz_logging,400, 50), SCHED_TASK(update_notify, 50, 90), SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(ekf_check, 10, 75), @@ -374,6 +375,15 @@ void Copter::update_batt_compass(void) } } +// Full rate logging of attitude, rate and pid loops +// should be run at 400hz +void Copter::fourhundred_hz_logging() +{ + if (should_log(MASK_LOG_ATTITUDE_FAST)) { + Log_Write_Attitude(); + } +} + // ten_hz_logging_loop // should be run at 10hz void Copter::ten_hz_logging_loop() @@ -381,13 +391,7 @@ void Copter::ten_hz_logging_loop() // log attitude data if we're not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control); - if (should_log(MASK_LOG_PID)) { - DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); - } + Log_Write_EKF_POS(); } if (should_log(MASK_LOG_MOTBATT)) { Log_Write_MotBatt(); @@ -427,14 +431,7 @@ void Copter::twentyfive_hz_logging() #if HIL_MODE == HIL_MODE_DISABLED if (should_log(MASK_LOG_ATTITUDE_FAST)) { - Log_Write_Attitude(); - DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control); - if (should_log(MASK_LOG_PID)) { - DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); - } + Log_Write_EKF_POS(); } // log IMU data if we're not already logging at the higher rate diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c42417af59..979e3a6f92 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -642,6 +642,7 @@ private: void update_mount(); void update_trigger(void); void update_batt_compass(void); + void fourhundred_hz_logging(); void ten_hz_logging_loop(); void twentyfive_hz_logging(); void three_hz_loop(); @@ -719,6 +720,7 @@ private: void Log_Write_Control_Tuning(); void Log_Write_Performance(); void Log_Write_Attitude(); + void Log_Write_EKF_POS(); void Log_Write_MotBatt(); void Log_Write_Event(uint8_t id); void Log_Write_Data(uint8_t id, int32_t value); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index c550122227..bc665448ea 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -376,7 +376,18 @@ void Copter::Log_Write_Attitude() Vector3f targets = attitude_control->get_att_target_euler_cd(); targets.z = wrap_360_cd(targets.z); DataFlash.Log_Write_Attitude(ahrs, targets); + DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control); + if (should_log(MASK_LOG_PID)) { + DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() ); + } +} +// Write an EKF and POS packet +void Copter::Log_Write_EKF_POS() +{ #if OPTFLOW == ENABLED DataFlash.Log_Write_EKF(ahrs,optflow.enabled()); #else @@ -958,6 +969,7 @@ void Copter::Log_Write_Nav_Tuning() {} void Copter::Log_Write_Control_Tuning() {} void Copter::Log_Write_Performance() {} void Copter::Log_Write_Attitude(void) {} +void Copter::Log_Write_EKF_POS() {} void Copter::Log_Write_MotBatt() {} void Copter::Log_Write_Event(uint8_t id) {} void Copter::Log_Write_Data(uint8_t id, int32_t value) {}