Copter: remove armed check before logging
This commit is contained in:
parent
84881470b1
commit
1793ee804e
@ -1099,7 +1099,7 @@ static void update_batt_compass(void)
|
||||
compass.null_offsets();
|
||||
}
|
||||
// log compass information
|
||||
if (motors.armed() && (g.log_bitmask & MASK_LOG_COMPASS)) {
|
||||
if (g.log_bitmask & MASK_LOG_COMPASS) {
|
||||
Log_Write_Compass();
|
||||
}
|
||||
}
|
||||
@ -1113,16 +1113,14 @@ static void update_batt_compass(void)
|
||||
// should be run at 10hz
|
||||
static void ten_hz_logging_loop()
|
||||
{
|
||||
if(motors.armed()) {
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {
|
||||
Log_Write_Attitude();
|
||||
}
|
||||
if (g.log_bitmask & MASK_LOG_RCIN) {
|
||||
DataFlash.Log_Write_RCIN();
|
||||
}
|
||||
if (g.log_bitmask & MASK_LOG_RCOUT) {
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
}
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {
|
||||
Log_Write_Attitude();
|
||||
}
|
||||
if (g.log_bitmask & MASK_LOG_RCIN) {
|
||||
DataFlash.Log_Write_RCIN();
|
||||
}
|
||||
if (g.log_bitmask & MASK_LOG_RCOUT) {
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
}
|
||||
}
|
||||
|
||||
@ -1135,13 +1133,14 @@ static void fifty_hz_logging_loop()
|
||||
gcs_send_message(MSG_RADIO_OUT);
|
||||
#endif
|
||||
|
||||
# if HIL_MODE == HIL_MODE_DISABLED
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) {
|
||||
#if HIL_MODE == HIL_MODE_DISABLED
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) {
|
||||
Log_Write_Attitude();
|
||||
}
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_IMU && motors.armed())
|
||||
if (g.log_bitmask & MASK_LOG_IMU) {
|
||||
DataFlash.Log_Write_IMU(ins);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -1180,8 +1179,9 @@ static void one_hz_loop()
|
||||
wp_nav.set_lean_angle_max(g.angle_max);
|
||||
|
||||
// log battery info to the dataflash
|
||||
if ((g.log_bitmask & MASK_LOG_CURRENT) && motors.armed())
|
||||
if (g.log_bitmask & MASK_LOG_CURRENT) {
|
||||
Log_Write_Current();
|
||||
}
|
||||
|
||||
// perform pre-arm checks & display failures every 30 seconds
|
||||
static uint8_t pre_arm_display_counter = 15;
|
||||
@ -1256,7 +1256,7 @@ static void update_GPS(void)
|
||||
last_gps_reading = g_gps->last_message_time_ms();
|
||||
|
||||
// log GPS message
|
||||
if ((g.log_bitmask & MASK_LOG_GPS) && motors.armed()) {
|
||||
if (g.log_bitmask & MASK_LOG_GPS) {
|
||||
DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
|
||||
}
|
||||
|
||||
@ -2092,7 +2092,7 @@ static void update_altitude()
|
||||
#endif // HIL_MODE == HIL_MODE_ATTITUDE
|
||||
|
||||
// write altitude info to dataflash logs
|
||||
if ((g.log_bitmask & MASK_LOG_CTUN) && motors.armed()) {
|
||||
if (g.log_bitmask & MASK_LOG_CTUN) {
|
||||
Log_Write_Control_Tuning();
|
||||
}
|
||||
}
|
||||
|
@ -8,7 +8,7 @@ static void read_inertia()
|
||||
// inertial altitude estimates
|
||||
inertial_nav.update(G_Dt);
|
||||
|
||||
if( motors.armed() && (g.log_bitmask & MASK_LOG_INAV) ) {
|
||||
if (g.log_bitmask & MASK_LOG_INAV) {
|
||||
log_counter_inav++;
|
||||
if( log_counter_inav >= 10 ) {
|
||||
log_counter_inav = 0;
|
||||
|
Loading…
Reference in New Issue
Block a user