Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 09:38:29 -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 977b8b5653
commit 2a34de7514
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
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

2
ArduCopter/Copter.h
View File

@ -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);

12
ArduCopter/Log.cpp
View File

@ -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) {}