Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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
SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
SCHED_TASK(compass_save, 0.1, 200, 114),
SCHED_TASK(Log_Write_Fast, 400, 300, 117),
SCHED_TASK(update_logging1, 25, 300, 120),
SCHED_TASK(update_logging2, 25, 300, 123),
SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
SCHED_TASK(update_logging10, 10, 300, 120),
SCHED_TASK(update_logging25, 25, 300, 123),
#if HAL_SOARING_ENABLED
SCHED_TASK(update_soaring, 50, 400, 126),
#endif
@ -226,24 +226,29 @@ void Plane::update_compass(void)
/*
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();
}
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();
} 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)) {
Log_Write_Control_Tuning();
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
void Plane::Log_Write_Fast(void)
void Plane::Log_Write_FullRate(void)
{
if (!should_log(MASK_LOG_ATTITUDE_FULLRATE)) {
uint32_t now = AP_HAL::millis();
if (now - last_log_fast_ms < 40) {
// default to 25Hz
return;
}
last_log_fast_ms = now;
}
if (should_log(MASK_LOG_ATTITUDE_FAST | MASK_LOG_ATTITUDE_FULLRATE)) {
// MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
// highest rate selected wins
if (should_log(MASK_LOG_ATTITUDE_FULLRATE)) {
Log_Write_Attitude();
}
}

6
ArduPlane/Plane.h
View File

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