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

AP_AHRS: move Log_Write_Home_And_Origin into AP_AHRS

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

23
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -17,6 +17,7 @@
#include "AP_AHRS.h"
#include "AP_AHRS_View.h"
#include <AP_HAL/AP_HAL.h>
#include <DataFlash/DataFlash.h>
extern const AP_HAL::HAL& hal;
@ -457,6 +458,28 @@ Vector2f AP_AHRS::rotate_body_to_earth2D(const Vector2f &bf) const
bf.x * _sin_yaw + bf.y * _cos_yaw);
}
// log ahrs home and EKF origin to dataflash
void AP_AHRS::Log_Write_Home_And_Origin()
{
DataFlash_Class *df = DataFlash_Class::instance();
if (df == nullptr) {
return;
}
#if AP_AHRS_NAVEKF_AVAILABLE
// log ekf origin if set
Location ekf_orig;
if (get_origin(ekf_orig)) {
df->Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
#endif
// log ahrs home if set
if (home_is_set()) {
df->Log_Write_Origin(LogOriginType::ahrs_home, _home);
}
}
// singleton instance
AP_AHRS *AP_AHRS::_singleton;

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -459,6 +459,8 @@ public:
// returns the inertial navigation origin in lat/lon/alt
virtual bool get_origin(Location &ret) const { return false; }
void Log_Write_Home_And_Origin();
// return true if the AHRS object supports inertial navigation,
// with very accurate position and velocity
virtual bool have_inertial_nav(void) const {