Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-23 02:55:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Copter: attitude logging at 400hz

Browse Source
This commit is contained in:
Leonard Hall
2017-06-22 15:18:04 +09:00
committed by Randy Mackay
parent 249849dfa4
commit 7eab1239c7
3 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduCopter/ArduCopter.cpp
+12 -15
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),
@@ -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
// should be run at 10hz
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
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();
@@ -417,14 +421,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
ArduCopter/Copter.h
+2
View File
@@ -653,6 +653,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();
@@ -729,6 +730,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);
ArduCopter/Log.cpp
+12
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
@@ -952,6 +963,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) {}