Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 04:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: attitude logging at 400hz

Browse Source
This commit is contained in:
Leonard Hall 2017-05-23 15:00:57 +09:30 committed by Randy Mackay
parent 249849dfa4
commit 7eab1239c7
3 changed files with 26 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduCopter/ArduCopter.cpp
View File

@ -109,6 +109,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75), SCHED_TASK(check_dynamic_flight, 50, 75),
#endif #endif
SCHED_TASK(fourhundred_hz_logging,400, 50),
SCHED_TASK(update_notify, 50, 90), SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75), SCHED_TASK(ekf_check, 10, 75),
@ -364,6 +365,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 // ten_hz_logging_loop
// should be run at 10hz // should be run at 10hz
void Copter::ten_hz_logging_loop() void Copter::ten_hz_logging_loop()
@ -371,13 +381,7 @@ void Copter::ten_hz_logging_loop()
// log attitude data if we're not already logging at the higher rate // 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)) { if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude(); Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control); Log_Write_EKF_POS();
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() );
}
} }
if (should_log(MASK_LOG_MOTBATT)) { if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt(); Log_Write_MotBatt();
@ -417,14 +421,7 @@ void Copter::twentyfive_hz_logging()
#if HIL_MODE == HIL_MODE_DISABLED #if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) { if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude(); Log_Write_EKF_POS();
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 IMU data if we're not already logging at the higher rate // log IMU data if we're not already logging at the higher rate

2
ArduCopter/Copter.h
View File

@ -653,6 +653,7 @@ private:
void update_mount(); void update_mount();
void update_trigger(void); void update_trigger(void);
void update_batt_compass(void); void update_batt_compass(void);
void fourhundred_hz_logging();
void ten_hz_logging_loop(); void ten_hz_logging_loop();
void twentyfive_hz_logging(); void twentyfive_hz_logging();
void three_hz_loop(); void three_hz_loop();
@ -729,6 +730,7 @@ private:
void Log_Write_Control_Tuning(); void Log_Write_Control_Tuning();
void Log_Write_Performance(); void Log_Write_Performance();
void Log_Write_Attitude(); void Log_Write_Attitude();
void Log_Write_EKF_POS();
void Log_Write_MotBatt(); void Log_Write_MotBatt();
void Log_Write_Event(uint8_t id); void Log_Write_Event(uint8_t id);
void Log_Write_Data(uint8_t id, int32_t value); void Log_Write_Data(uint8_t id, int32_t value);

12
ArduCopter/Log.cpp
View File

@ -376,7 +376,18 @@ void Copter::Log_Write_Attitude()
Vector3f targets = attitude_control->get_att_target_euler_cd(); Vector3f targets = attitude_control->get_att_target_euler_cd();
targets.z = wrap_360_cd(targets.z); targets.z = wrap_360_cd(targets.z);
DataFlash.Log_Write_Attitude(ahrs, targets); 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 #if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled()); DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else #else
@ -952,6 +963,7 @@ void Copter::Log_Write_Nav_Tuning() {}
void Copter::Log_Write_Control_Tuning() {} void Copter::Log_Write_Control_Tuning() {}
void Copter::Log_Write_Performance() {} void Copter::Log_Write_Performance() {}
void Copter::Log_Write_Attitude(void) {} void Copter::Log_Write_Attitude(void) {}
void Copter::Log_Write_EKF_POS() {}
void Copter::Log_Write_MotBatt() {} void Copter::Log_Write_MotBatt() {}
void Copter::Log_Write_Event(uint8_t id) {} void Copter::Log_Write_Event(uint8_t id) {}
void Copter::Log_Write_Data(uint8_t id, int32_t value) {} void Copter::Log_Write_Data(uint8_t id, int32_t value) {}