Browse Source

Copter: fix startup logging

Removed potentially endless loop caused by start_logging calling
Log_Write_Startup which called should_log which could then call
start_logging.
Moved disarm event logging above motors disarm so it is logged
mission-4.1.18
Randy Mackay 10 years ago
parent
commit
4e3d0ae0c1
  1. 10
      ArduCopter/Log.cpp
  2. 12
      ArduCopter/motors.cpp
  3. 4
      ArduCopter/system.cpp

10
ArduCopter/Log.cpp

@ -448,11 +448,6 @@ void Copter::Log_Write_Startup() @@ -448,11 +448,6 @@ void Copter::Log_Write_Startup()
time_us : hal.scheduler->micros64()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
// write all commands to the dataflash as well
if (should_log(MASK_LOG_CMD)) {
DataFlash.Log_Write_EntireMission(mission);
}
}
struct PACKED log_Event {
@ -755,6 +750,11 @@ void Copter::start_logging() @@ -755,6 +750,11 @@ void Copter::start_logging()
DataFlash.Log_Write_Message_P(PSTR("Frame: " FRAME_CONFIG_STRING));
// write mission commands
if (MASK_LOG_CMD & g.log_bitmask) {
DataFlash.Log_Write_EntireMission(mission);
}
Log_Write_Startup();
// log the flight mode

12
ArduCopter/motors.cpp

@ -822,9 +822,6 @@ void Copter::init_disarm_motors() @@ -822,9 +822,6 @@ void Copter::init_disarm_motors()
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif
// send disarm command to motors
motors.armed(false);
// save compass offsets learned by the EKF
Vector3f magOffsets;
if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
@ -840,12 +837,15 @@ void Copter::init_disarm_motors() @@ -840,12 +837,15 @@ void Copter::init_disarm_motors()
set_land_complete(true);
set_land_complete_maybe(true);
// reset the mission
mission.reset();
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
// suspend logging
if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) {
DataFlash.EnableWrites(false);

4
ArduCopter/system.cpp

@ -255,10 +255,6 @@ void Copter::init_ardupilot() @@ -255,10 +255,6 @@ void Copter::init_ardupilot()
startup_ground(true);
#if LOGGING_ENABLED == ENABLED
Log_Write_Startup();
#endif
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly

Loading…
Cancel
Save