Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fix attitude/AOA logging and rates

Browse Source
This commit is contained in:
Henry Wurzburg 2022-07-28 21:42:21 -05:00 committed by Andrew Tridgell
parent 223b005d29
commit fcf29539de
3 changed files with 25 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduPlane/ArduPlane.cpp
View File

@ -101,9 +101,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
#endif // CAMERA == ENABLED #endif // CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111), SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
SCHED_TASK(compass_save, 0.1, 200, 114), SCHED_TASK(compass_save, 0.1, 200, 114),
SCHED_TASK(Log_Write_Fast, 400, 300, 117), SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
SCHED_TASK(update_logging1, 25, 300, 120), SCHED_TASK(update_logging10, 10, 300, 120),
SCHED_TASK(update_logging2, 25, 300, 123), SCHED_TASK(update_logging25, 25, 300, 123),
#if HAL_SOARING_ENABLED #if HAL_SOARING_ENABLED
SCHED_TASK(update_soaring, 50, 400, 126), SCHED_TASK(update_soaring, 50, 400, 126),
#endif #endif
@ -226,24 +226,29 @@ void Plane::update_compass(void)
/* /*
do 10Hz logging do 10Hz logging
*/ */
void Plane::update_logging1(void) void Plane::update_logging10(void)
{ {
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST));
if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) {
Log_Write_Attitude(); Log_Write_Attitude();
}
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
AP::ins().Write_IMU();
if (should_log(MASK_LOG_ATTITUDE_MED))
ahrs.Write_AOA_SSA(); ahrs.Write_AOA_SSA();
} else if (log_faster) {
ahrs.Write_AOA_SSA();
}
} }
/* /*
do 10Hz logging - part2 do 25Hz logging
*/ */
void Plane::update_logging2(void) void Plane::update_logging25(void)
{ {
// MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
// highest rate selected wins
bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE);
if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_CTUN)) { if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
AP::ins().write_notch_log_messages(); AP::ins().write_notch_log_messages();

14
ArduPlane/Log.cpp
View File

@ -49,17 +49,11 @@ void Plane::Log_Write_Attitude(void)
} }
// do fast logging for plane // do fast logging for plane
void Plane::Log_Write_Fast(void) void Plane::Log_Write_FullRate(void)
{ {
if (!should_log(MASK_LOG_ATTITUDE_FULLRATE)) { // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
uint32_t now = AP_HAL::millis(); // highest rate selected wins
if (now - last_log_fast_ms < 40) { if (should_log(MASK_LOG_ATTITUDE_FULLRATE)) {
// default to 25Hz
return;
}
last_log_fast_ms = now;
}
if (should_log(MASK_LOG_ATTITUDE_FAST | MASK_LOG_ATTITUDE_FULLRATE)) {
Log_Write_Attitude(); Log_Write_Attitude();
} }
} }

6
ArduPlane/Plane.h
View File

@ -868,7 +868,7 @@ private:
// Log.cpp // Log.cpp
uint32_t last_log_fast_ms; uint32_t last_log_fast_ms;
void Log_Write_Fast(void); void Log_Write_FullRate(void);
void Log_Write_Attitude(void); void Log_Write_Attitude(void);
void Log_Write_Control_Tuning(); void Log_Write_Control_Tuning();
void Log_Write_OFG_Guided(); void Log_Write_OFG_Guided();
@ -1006,8 +1006,8 @@ private:
void airspeed_ratio_update(void); void airspeed_ratio_update(void);
#endif #endif
void compass_save(void); void compass_save(void);
void update_logging1(void); void update_logging10(void);
void update_logging2(void); void update_logging25(void);
void update_control_mode(void); void update_control_mode(void);
void update_fly_forward(void); void update_fly_forward(void);
void update_flight_stage(); void update_flight_stage();