AP_AHRS: make AHRS handle altitude
AHRS now holds the home position
This commit is contained in:
parent
b9128a932f
commit
392995ef84
@ -228,26 +228,6 @@ Vector2f AP_AHRS::groundspeed_vector(void)
|
||||
return Vector2f(0.0f, 0.0f);
|
||||
}
|
||||
|
||||
/*
|
||||
get position projected by groundspeed and heading
|
||||
*/
|
||||
bool AP_AHRS::get_projected_position(struct Location &loc)
|
||||
{
|
||||
if (!get_position(loc)) {
|
||||
return false;
|
||||
}
|
||||
location_update(loc, degrees(yaw), _gps->ground_speed_cm * 0.01 * _gps->get_lag());
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
get the GPS lag in seconds
|
||||
*/
|
||||
float AP_AHRS::get_position_lag(void) const
|
||||
{
|
||||
return _gps->get_lag();
|
||||
}
|
||||
|
||||
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
||||
// should be called after _dcm_matrix is updated
|
||||
void AP_AHRS::update_trig(void)
|
||||
|
@ -61,6 +61,14 @@ public:
|
||||
|
||||
// enable centrifugal correction by default
|
||||
_flags.correct_centrifugal = true;
|
||||
|
||||
// initialise _home
|
||||
_home.id = MAV_CMD_NAV_WAYPOINT;
|
||||
_home.options = 0;
|
||||
_home.p1 = 0;
|
||||
_home.alt = 0;
|
||||
_home.lng = 0;
|
||||
_home.lat = 0;
|
||||
}
|
||||
|
||||
// init sets up INS board orientation
|
||||
@ -143,12 +151,6 @@ public:
|
||||
// reset the current attitude, used on new IMU calibration
|
||||
virtual void reset_attitude(const float &roll, const float &pitch, const float &yaw) = 0;
|
||||
|
||||
// how often our attitude representation has gone out of range
|
||||
uint8_t renorm_range_count;
|
||||
|
||||
// how often our attitude representation has blown up completely
|
||||
uint8_t renorm_blowup_count;
|
||||
|
||||
// return the average size of the roll/pitch error estimate
|
||||
// since last call
|
||||
virtual float get_error_rp(void) = 0;
|
||||
@ -161,30 +163,12 @@ public:
|
||||
// attitude
|
||||
virtual const Matrix3f &get_dcm_matrix(void) const = 0;
|
||||
|
||||
// get our current position, either from GPS or via
|
||||
// dead-reckoning. Return true if a position is available,
|
||||
// otherwise false. This only updates the lat and lng fields
|
||||
// of the Location
|
||||
virtual bool get_position(struct Location &loc) {
|
||||
if (!_gps || _gps->status() <= GPS::NO_FIX) {
|
||||
return false;
|
||||
}
|
||||
loc.lat = _gps->latitude;
|
||||
loc.lng = _gps->longitude;
|
||||
return true;
|
||||
}
|
||||
|
||||
// get our projected position, based on our GPS position plus
|
||||
// heading and ground speed
|
||||
bool get_projected_position(struct Location &loc);
|
||||
|
||||
// return the estimated lag in our position due to GPS lag
|
||||
float get_position_lag(void) const;
|
||||
// get our current position estimate. Return true if a position is available,
|
||||
// otherwise false. This call fills in lat, lng and alt
|
||||
virtual bool get_position(struct Location &loc) = 0;
|
||||
|
||||
// return a wind estimation vector, in m/s
|
||||
virtual Vector3f wind_estimate(void) {
|
||||
return Vector3f(0,0,0);
|
||||
}
|
||||
virtual Vector3f wind_estimate(void) = 0;
|
||||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate
|
||||
@ -275,6 +259,15 @@ public:
|
||||
// return secondary position solution if available
|
||||
virtual bool get_secondary_position(struct Location &loc) { return false; }
|
||||
|
||||
// get the home location. This is const to prevent any changes to
|
||||
// home without telling AHRS about the change
|
||||
const struct Location &get_home(void) const { return _home; }
|
||||
|
||||
// set the home location in 10e7 degrees. This should be called
|
||||
// when the vehicle is at this position. It is assumed that the
|
||||
// current barometer and GPS altitudes correspond to this altitude
|
||||
virtual void set_home(int32_t lat, int32_t lon, int32_t alt_cm) = 0;
|
||||
|
||||
protected:
|
||||
// settable parameters
|
||||
AP_Float beta;
|
||||
@ -329,6 +322,9 @@ protected:
|
||||
Vector2f _hp; // ground vector high-pass filter
|
||||
Vector2f _lastGndVelADS; // previous HPF input
|
||||
|
||||
// reference position for NED positions
|
||||
struct Location _home;
|
||||
|
||||
// helper trig variables
|
||||
float _cos_roll, _cos_pitch, _cos_yaw;
|
||||
float _sin_roll, _sin_pitch, _sin_yaw;
|
||||
|
@ -134,7 +134,6 @@ AP_AHRS_DCM::check_matrix(void)
|
||||
{
|
||||
if (_dcm_matrix.is_nan()) {
|
||||
//Serial.printf("ERROR: DCM matrix NAN\n");
|
||||
renorm_blowup_count++;
|
||||
reset(true);
|
||||
return;
|
||||
}
|
||||
@ -145,7 +144,6 @@ AP_AHRS_DCM::check_matrix(void)
|
||||
if (!(_dcm_matrix.c.x < 1.0f &&
|
||||
_dcm_matrix.c.x > -1.0f)) {
|
||||
// We have an invalid matrix. Force a normalisation.
|
||||
renorm_range_count++;
|
||||
normalize();
|
||||
|
||||
if (_dcm_matrix.is_nan() ||
|
||||
@ -154,7 +152,6 @@ AP_AHRS_DCM::check_matrix(void)
|
||||
// in real trouble. All we can do is reset
|
||||
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
|
||||
// _dcm_matrix.c.x);
|
||||
renorm_blowup_count++;
|
||||
reset(true);
|
||||
}
|
||||
}
|
||||
@ -193,7 +190,6 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
|
||||
|
||||
if (!(renorm_val < 2.0f && renorm_val > 0.5f)) {
|
||||
// this is larger than it should get - log it as a warning
|
||||
renorm_range_count++;
|
||||
if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) {
|
||||
// we are getting values which are way out of
|
||||
// range, we will reset the matrix and hope we
|
||||
@ -201,7 +197,6 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
|
||||
// correction before we hit the ground!
|
||||
//Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
|
||||
// renorm_val);
|
||||
renorm_blowup_count++;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@ -854,7 +849,11 @@ bool AP_AHRS_DCM::get_position(struct Location &loc)
|
||||
}
|
||||
loc.lat = _last_lat;
|
||||
loc.lng = _last_lng;
|
||||
loc.alt = _baro.get_altitude() * 100 + _home.alt;
|
||||
location_offset(loc, _position_offset_north, _position_offset_east);
|
||||
if (_flags.fly_forward) {
|
||||
location_update(loc, degrees(yaw), _gps->ground_speed_cm * 0.01 * _gps->get_lag());
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -889,3 +888,10 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void AP_AHRS_DCM::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
|
||||
{
|
||||
_home.lat = lat;
|
||||
_home.lng = lng;
|
||||
_home.alt = alt_cm;
|
||||
}
|
||||
|
@ -78,6 +78,8 @@ public:
|
||||
|
||||
bool use_compass(void);
|
||||
|
||||
void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
|
||||
|
||||
private:
|
||||
float _ki;
|
||||
float _ki_yaw;
|
||||
|
@ -187,4 +187,12 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
|
||||
return Vector2f(vec.x, vec.y);
|
||||
}
|
||||
|
||||
void AP_AHRS_NavEKF::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
|
||||
{
|
||||
AP_AHRS_DCM::set_home(lat, lng, alt_cm);
|
||||
if (ekf_started) {
|
||||
EKF.InitialiseFilter();
|
||||
}
|
||||
}
|
||||
|
||||
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
||||
|
@ -82,6 +82,9 @@ public:
|
||||
// EKF has a better ground speed vector estimate
|
||||
Vector2f groundspeed_vector(void);
|
||||
|
||||
// set home location
|
||||
void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
|
||||
|
||||
private:
|
||||
bool using_EKF(void) { return ekf_started && _ekf_use && EKF.healthy(); }
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user