|
|
|
@ -330,7 +330,7 @@ void Copter::update_batt_compass(void)
@@ -330,7 +330,7 @@ void Copter::update_batt_compass(void)
|
|
|
|
|
// should be run at 400hz
|
|
|
|
|
void Copter::fourhundred_hz_logging() |
|
|
|
|
{ |
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST)) { |
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->stop_attitude_logging()) { |
|
|
|
|
Log_Write_Attitude(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -340,8 +340,11 @@ void Copter::fourhundred_hz_logging()
@@ -340,8 +340,11 @@ void Copter::fourhundred_hz_logging()
|
|
|
|
|
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)) { |
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->stop_attitude_logging()) { |
|
|
|
|
Log_Write_Attitude(); |
|
|
|
|
} |
|
|
|
|
// log EKF attitude data
|
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) { |
|
|
|
|
Log_Write_EKF_POS(); |
|
|
|
|
} |
|
|
|
|
if (should_log(MASK_LOG_MOTBATT)) { |
|
|
|
|