aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: log ahrs home and ekf origin

Browse Source
This commit is contained in:
Randy Mackay 2015-07-05 17:25:28 +09:00
parent ad1f9c4829
commit 221ddb545e
3 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -597,6 +597,7 @@ private:
void Log_Write_Error(uint8_t sub_system, uint8_t error_code); void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
void Log_Write_Baro(void); void Log_Write_Baro(void);
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high); void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high);
void Log_Write_Home_And_Origin();
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void); void Log_Write_Heli(void);
#endif #endif

15
ArduCopter/Log.cpp
View File

@ -631,6 +631,21 @@ void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t
DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune)); DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
} }
// log EKF origin and ahrs home to dataflash
void Copter::Log_Write_Home_And_Origin()
{
// log ekf origin if set
Location ekf_orig;
if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) {
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
// log ahrs home if set
if (ap.home_state != HOME_UNSET) {
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
}
}
struct PACKED log_Heli { struct PACKED log_Heli {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;

3
ArduCopter/commands.cpp
View File

@ -101,6 +101,9 @@ bool Copter::set_home(const Location& loc)
} }
} }
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// return success // return success
return true; return true;
} }