mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
Arducopter: Change mode logging to use common-vehicle DataFlash library.
This commit is contained in:
parent
2657610373
commit
4b86a4aeec
@ -397,23 +397,6 @@ static void Log_Write_Attitude()
|
||||
#endif
|
||||
}
|
||||
|
||||
struct PACKED log_Mode {
|
||||
LOG_PACKET_HEADER;
|
||||
uint8_t mode;
|
||||
int16_t throttle_cruise;
|
||||
};
|
||||
|
||||
// Write a mode packet
|
||||
static void Log_Write_Mode(uint8_t mode)
|
||||
{
|
||||
struct log_Mode pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
|
||||
mode : mode,
|
||||
throttle_cruise : g.throttle_cruise,
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Startup {
|
||||
LOG_PACKET_HEADER;
|
||||
};
|
||||
@ -577,8 +560,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
|
||||
"CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
|
||||
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
|
||||
"PM", "HHIhBHB", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" },
|
||||
{ LOG_MODE_MSG, sizeof(log_Mode),
|
||||
"MODE", "Mh", "Mode,ThrCrs" },
|
||||
{ LOG_STARTUP_MSG, sizeof(log_Startup),
|
||||
"STRT", "", "" },
|
||||
{ LOG_EVENT_MSG, sizeof(log_Event),
|
||||
@ -637,7 +618,7 @@ static void start_logging()
|
||||
DataFlash.Log_Write_Message_P(PSTR("Frame: " FRAME_CONFIG_STRING));
|
||||
|
||||
// log the flight mode
|
||||
Log_Write_Mode(control_mode);
|
||||
DataFlash.Log_Write_Mode(control_mode);
|
||||
}
|
||||
// enable writes
|
||||
DataFlash.EnableWrites(true);
|
||||
|
@ -218,7 +218,6 @@ enum FlipState {
|
||||
// Logging parameters
|
||||
#define TYPE_AIRSTART_MSG 0x00
|
||||
#define TYPE_GROUNDSTART_MSG 0x01
|
||||
#define LOG_MODE_MSG 0x03
|
||||
#define LOG_CONTROL_TUNING_MSG 0x04
|
||||
#define LOG_NAV_TUNING_MSG 0x05
|
||||
#define LOG_PERFORMANCE_MSG 0x06
|
||||
|
@ -99,7 +99,7 @@ static bool set_mode(uint8_t mode)
|
||||
// perform any cleanup required by previous flight mode
|
||||
exit_mode(control_mode, mode);
|
||||
control_mode = mode;
|
||||
Log_Write_Mode(control_mode);
|
||||
DataFlash.Log_Write_Mode(control_mode);
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
|
||||
|
@ -189,7 +189,7 @@ static bool init_arm_motors()
|
||||
Log_Write_Event(DATA_ARMED);
|
||||
|
||||
// log flight mode in case it was changed while vehicle was disarmed
|
||||
Log_Write_Mode(control_mode);
|
||||
DataFlash.Log_Write_Mode(control_mode);
|
||||
|
||||
// reenable failsafe
|
||||
failsafe_enable();
|
||||
|
Loading…
Reference in New Issue
Block a user