Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: log ahrs home and ekf origin

This commit is contained in:
Randy Mackay 2015-07-06 13:17:05 +09:00
parent 76ccf4043e
commit 5ef4f8e90a
4 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
APMrover2/Log.cpp
View File

@ -359,6 +359,23 @@ void Rover::Log_Write_Baro(void)
DataFlash.Log_Write_Baro(barometer);
}
// log ahrs home and EKF origin to dataflash
void Rover::Log_Write_Home_And_Origin()
{
#if AP_AHRS_NAVEKF_AVAILABLE
// 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);
}
#endif
// log ahrs home if set
if (home_is_set) {
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
}
}
const LogStructure Rover::log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES,
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),

1
APMrover2/Rover.h
View File

@ -405,6 +405,7 @@ private:
void Log_Write_Attitude();
void Log_Write_RC(void);
void Log_Write_Baro(void);
void Log_Write_Home_And_Origin();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void log_init(void);
void start_logging() ;

1
APMrover2/commands.cpp
View File

@ -67,6 +67,7 @@ void Rover::init_home()
ahrs.set_home(gps.location());
home_is_set = true;
Log_Write_Home_And_Origin();
// Save Home to EEPROM
mission.write_home_to_storage();

1
APMrover2/commands_logic.cpp
View File

@ -282,6 +282,7 @@ void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
} else {
ahrs.set_home(cmd.content.location);
home_is_set = true;
Log_Write_Home_And_Origin();
}
}