Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: move Log_Write_Home_And_Origin into AP_AHRS

Browse Source
This commit is contained in:
Peter Barker 2018-05-16 13:28:59 +10:00 committed by Andrew Tridgell
parent 9793703a76
commit a52f3d8f70
3 changed files with 4 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
APMrover2/Log.cpp
View File

@ -204,23 +204,6 @@ void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
// 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_origin(ekf_orig)) {
DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
#endif
// log ahrs home if set
if (ahrs.home_is_set()) {
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
}
}
// guided mode logging // guided mode logging
struct PACKED log_GuidedTarget { struct PACKED log_GuidedTarget {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
@ -324,7 +307,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages()
// only 200(?) bytes are guaranteed by DataFlash // only 200(?) bytes are guaranteed by DataFlash
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason); DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
Log_Write_Home_And_Origin(); ahrs.Log_Write_Home_And_Origin();
gps.Write_DataFlash_Log_Startup_messages(); gps.Write_DataFlash_Log_Startup_messages();
} }
@ -339,7 +322,6 @@ void Rover::Log_Write_Rangefinder() {}
void Rover::Log_Write_Attitude() {} void Rover::Log_Write_Attitude() {}
void Rover::Log_Write_RC(void) {} void Rover::Log_Write_RC(void) {}
void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Rover::Log_Write_Home_And_Origin() {}
void Rover::Log_Arm_Disarm() {} void Rover::Log_Arm_Disarm() {}
void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {} void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Rover::Log_Write_Steering() {} void Rover::Log_Write_Steering() {}

1
APMrover2/Rover.h
View File

@ -490,7 +490,6 @@ private:
void Log_Arm_Disarm(); void Log_Arm_Disarm();
void Log_Write_RC(void); void Log_Write_RC(void);
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_Home_And_Origin();
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_WheelEncoder(); void Log_Write_WheelEncoder();
void Log_Write_Proximity(); void Log_Write_Proximity();

6
APMrover2/commands.cpp
View File

@ -72,7 +72,7 @@ bool Rover::set_home(const Location& loc, bool lock)
mission.write_home_to_storage(); mission.write_home_to_storage();
// log ahrs home and ekf origin dataflash // log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin(); ahrs.Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS // send new home and ekf origin to GCS
gcs().send_home(loc); gcs().send_home(loc);
@ -108,7 +108,7 @@ void Rover::set_ekf_origin(const Location& loc)
} }
// log ahrs home and ekf origin dataflash // log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin(); ahrs.Log_Write_Home_And_Origin();
// send ekf origin to GCS // send ekf origin to GCS
gcs().send_ekf_origin(loc); gcs().send_ekf_origin(loc);
@ -148,7 +148,7 @@ void Rover::update_home()
if (ahrs.get_position(loc)) { if (ahrs.get_position(loc)) {
if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) { if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {
ahrs.set_home(loc); ahrs.set_home(loc);
Log_Write_Home_And_Origin(); ahrs.Log_Write_Home_And_Origin();
gcs().send_home(gps.location()); gcs().send_home(gps.location());
} }
} }