From 392995ef84a55202382126a235a7e9f4ef30f5b7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Jan 2014 22:06:10 +1100 Subject: [PATCH] AP_AHRS: make AHRS handle altitude AHRS now holds the home position --- libraries/AP_AHRS/AP_AHRS.cpp | 20 ----------- libraries/AP_AHRS/AP_AHRS.h | 52 +++++++++++++--------------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 16 ++++++--- libraries/AP_AHRS/AP_AHRS_DCM.h | 2 ++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 8 +++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 ++ 6 files changed, 48 insertions(+), 53 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 8780e27e19..317ef864b2 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -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) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index b797bcf830..0d09bf79a1 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 4990b91854..c4f29bd0a5 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -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; +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 2c6cb3eb2e..b2476f2fb0 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -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; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index f7648f1505..56b1acba86 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -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 diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 2b6565fbd2..9367723685 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -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(); }