mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: added support for AP_AHRS_View
This commit is contained in:
parent
1345bf8737
commit
c62c64d27b
|
@ -15,7 +15,9 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
#include "AP_AHRS.h"
|
#include "AP_AHRS.h"
|
||||||
|
#include "AP_AHRS_View.h"
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// table of user settable parameters
|
// table of user settable parameters
|
||||||
|
@ -229,6 +231,55 @@ Vector2f AP_AHRS::groundspeed_vector(void)
|
||||||
return Vector2f(0.0f, 0.0f);
|
return Vector2f(0.0f, 0.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
|
||||||
|
*/
|
||||||
|
void AP_AHRS::calc_trig(const Matrix3f &rot,
|
||||||
|
float &cr, float &cp, float &cy,
|
||||||
|
float &sr, float &sp, float &sy) const
|
||||||
|
{
|
||||||
|
Vector2f yaw_vector;
|
||||||
|
|
||||||
|
yaw_vector.x = rot.a.x;
|
||||||
|
yaw_vector.y = rot.b.x;
|
||||||
|
if (fabsf(yaw_vector.x) > 0 ||
|
||||||
|
fabsf(yaw_vector.y) > 0) {
|
||||||
|
yaw_vector.normalize();
|
||||||
|
}
|
||||||
|
sy = constrain_float(yaw_vector.y, -1.0, 1.0);
|
||||||
|
cy = constrain_float(yaw_vector.x, -1.0, 1.0);
|
||||||
|
|
||||||
|
// sanity checks
|
||||||
|
if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
|
||||||
|
sy = 0.0f;
|
||||||
|
cy = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float cx2 = rot.c.x * rot.c.x;
|
||||||
|
if (cx2 >= 1.0f) {
|
||||||
|
cp = 0;
|
||||||
|
cr = 1.0f;
|
||||||
|
} else {
|
||||||
|
cp = safe_sqrt(1 - cx2);
|
||||||
|
cr = rot.c.z / cp;
|
||||||
|
}
|
||||||
|
cp = constrain_float(cp, 0, 1.0);
|
||||||
|
cr = constrain_float(cr, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing
|
||||||
|
|
||||||
|
sp = -rot.c.x;
|
||||||
|
|
||||||
|
if (!is_zero(cp)) {
|
||||||
|
sr = rot.c.y / cp;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) {
|
||||||
|
float r, p, y;
|
||||||
|
rot.to_euler(&r, &p, &y);
|
||||||
|
cr = cosf(r);
|
||||||
|
sr = sinf(r);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
||||||
// should be called after _dcm_matrix is updated
|
// should be called after _dcm_matrix is updated
|
||||||
void AP_AHRS::update_trig(void)
|
void AP_AHRS::update_trig(void)
|
||||||
|
@ -239,51 +290,9 @@ void AP_AHRS::update_trig(void)
|
||||||
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
|
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector2f yaw_vector;
|
calc_trig(get_rotation_body_to_ned(),
|
||||||
const Matrix3f &temp = get_rotation_body_to_ned();
|
_cos_roll, _cos_pitch, _cos_yaw,
|
||||||
|
_sin_roll, _sin_pitch, _sin_yaw);
|
||||||
// sin_yaw, cos_yaw
|
|
||||||
yaw_vector.x = temp.a.x;
|
|
||||||
yaw_vector.y = temp.b.x;
|
|
||||||
yaw_vector.normalize();
|
|
||||||
_sin_yaw = constrain_float(yaw_vector.y, -1.0, 1.0);
|
|
||||||
_cos_yaw = constrain_float(yaw_vector.x, -1.0, 1.0);
|
|
||||||
|
|
||||||
// cos_roll, cos_pitch
|
|
||||||
float cx2 = temp.c.x * temp.c.x;
|
|
||||||
if (cx2 >= 1.0f) {
|
|
||||||
_cos_pitch = 0;
|
|
||||||
_cos_roll = 1.0f;
|
|
||||||
} else {
|
|
||||||
_cos_pitch = safe_sqrt(1 - cx2);
|
|
||||||
_cos_roll = temp.c.z / _cos_pitch;
|
|
||||||
}
|
|
||||||
_cos_pitch = constrain_float(_cos_pitch, 0, 1.0);
|
|
||||||
_cos_roll = constrain_float(_cos_roll, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing
|
|
||||||
|
|
||||||
// sin_roll, sin_pitch
|
|
||||||
_sin_pitch = -temp.c.x;
|
|
||||||
if (is_zero(_cos_pitch)) {
|
|
||||||
_sin_roll = sinf(roll);
|
|
||||||
} else {
|
|
||||||
_sin_roll = temp.c.y / _cos_pitch;
|
|
||||||
}
|
|
||||||
|
|
||||||
// sanity checks
|
|
||||||
if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
|
|
||||||
yaw_vector.x = 0.0f;
|
|
||||||
yaw_vector.y = 0.0f;
|
|
||||||
_sin_yaw = 0.0f;
|
|
||||||
_cos_yaw = 1.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (isinf(_cos_roll) || isnan(_cos_roll)) {
|
|
||||||
_cos_roll = cosf(roll);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (isinf(_sin_roll) || isnan(_sin_roll)) {
|
|
||||||
_sin_roll = sinf(roll);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -297,3 +306,16 @@ void AP_AHRS::update_cd_values(void)
|
||||||
if (yaw_sensor < 0)
|
if (yaw_sensor < 0)
|
||||||
yaw_sensor += 36000;
|
yaw_sensor += 36000;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
create a rotated view of AP_AHRS
|
||||||
|
*/
|
||||||
|
AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation)
|
||||||
|
{
|
||||||
|
if (_view != nullptr) {
|
||||||
|
// can only have one
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
_view = new AP_AHRS_View(*this, rotation);
|
||||||
|
return _view;
|
||||||
|
}
|
||||||
|
|
|
@ -43,9 +43,14 @@ enum AHRS_VehicleClass {
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// forward declare view class
|
||||||
|
class AP_AHRS_View;
|
||||||
|
|
||||||
class AP_AHRS
|
class AP_AHRS
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
friend class AP_AHRS_View;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
|
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
|
||||||
roll(0.0f),
|
roll(0.0f),
|
||||||
|
@ -499,6 +504,9 @@ public:
|
||||||
|
|
||||||
// Retrieves the corrected NED delta velocity in use by the inertial navigation
|
// Retrieves the corrected NED delta velocity in use by the inertial navigation
|
||||||
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); }
|
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); }
|
||||||
|
|
||||||
|
// create a view
|
||||||
|
AP_AHRS_View *create_view(enum Rotation rotation);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
AHRS_VehicleClass _vehicle_class;
|
AHRS_VehicleClass _vehicle_class;
|
||||||
|
@ -525,6 +533,11 @@ protected:
|
||||||
uint8_t wind_estimation : 1; // 1 if we should do wind estimation
|
uint8_t wind_estimation : 1; // 1 if we should do wind estimation
|
||||||
} _flags;
|
} _flags;
|
||||||
|
|
||||||
|
// calculate sin/cos of roll/pitch/yaw from rotation
|
||||||
|
void calc_trig(const Matrix3f &rot,
|
||||||
|
float &cr, float &cp, float &cy,
|
||||||
|
float &sr, float &sp, float &sy) const;
|
||||||
|
|
||||||
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
||||||
// should be called after _dcm_matrix is updated
|
// should be called after _dcm_matrix is updated
|
||||||
void update_trig(void);
|
void update_trig(void);
|
||||||
|
@ -584,6 +597,9 @@ protected:
|
||||||
|
|
||||||
// which accelerometer instance is active
|
// which accelerometer instance is active
|
||||||
uint8_t _active_accel_instance;
|
uint8_t _active_accel_instance;
|
||||||
|
|
||||||
|
// optional view class
|
||||||
|
AP_AHRS_View *_view;
|
||||||
};
|
};
|
||||||
|
|
||||||
#include "AP_AHRS_DCM.h"
|
#include "AP_AHRS_DCM.h"
|
||||||
|
|
|
@ -20,6 +20,7 @@
|
||||||
*/
|
*/
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include "AP_AHRS.h"
|
#include "AP_AHRS.h"
|
||||||
|
#include "AP_AHRS_View.h"
|
||||||
#include <AP_Vehicle/AP_Vehicle.h>
|
#include <AP_Vehicle/AP_Vehicle.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
#include <AP_Module/AP_Module.h>
|
#include <AP_Module/AP_Module.h>
|
||||||
|
@ -99,6 +100,11 @@ void AP_AHRS_NavEKF::update(void)
|
||||||
const Vector3f &exported_gyro_bias = get_gyro_drift();
|
const Vector3f &exported_gyro_bias = get_gyro_drift();
|
||||||
hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
|
hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (_view != nullptr) {
|
||||||
|
// update optional alternative attitude view
|
||||||
|
_view->update();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_AHRS_NavEKF::update_DCM(void)
|
void AP_AHRS_NavEKF::update_DCM(void)
|
||||||
|
|
|
@ -19,12 +19,28 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "AP_AHRS_View.h"
|
#include "AP_AHRS_View.h"
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) :
|
AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) :
|
||||||
rotation(_rotation),
|
rotation(_rotation),
|
||||||
ahrs(_ahrs)
|
ahrs(_ahrs)
|
||||||
{
|
{
|
||||||
|
switch (rotation) {
|
||||||
|
case ROTATION_NONE:
|
||||||
|
rot_view.identity();
|
||||||
|
break;
|
||||||
|
case ROTATION_PITCH_90:
|
||||||
|
rot_view.from_euler(0, radians(90), 0);
|
||||||
|
break;
|
||||||
|
case ROTATION_PITCH_270:
|
||||||
|
rot_view.from_euler(0, radians(270), 0);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
|
||||||
|
}
|
||||||
|
|
||||||
|
// setup initial state
|
||||||
|
update();
|
||||||
}
|
}
|
||||||
|
|
||||||
// update state
|
// update state
|
||||||
|
@ -33,6 +49,14 @@ void AP_AHRS_View::update(void)
|
||||||
rot_body_to_ned = ahrs.get_rotation_body_to_ned();
|
rot_body_to_ned = ahrs.get_rotation_body_to_ned();
|
||||||
gyro = ahrs.get_gyro();
|
gyro = ahrs.get_gyro();
|
||||||
|
|
||||||
|
if (rotation != ROTATION_NONE) {
|
||||||
|
Matrix3f &r = rot_body_to_ned;
|
||||||
|
r.transpose();
|
||||||
|
r = rot_view * r;
|
||||||
|
r.transpose();
|
||||||
|
gyro.rotate(rotation);
|
||||||
|
}
|
||||||
|
|
||||||
rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
|
rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
|
||||||
|
|
||||||
roll_sensor = degrees(roll) * 100;
|
roll_sensor = degrees(roll) * 100;
|
||||||
|
|
|
@ -30,7 +30,7 @@ public:
|
||||||
|
|
||||||
// update state
|
// update state
|
||||||
void update(void);
|
void update(void);
|
||||||
|
|
||||||
// empty virtual destructor
|
// empty virtual destructor
|
||||||
virtual ~AP_AHRS_View() {}
|
virtual ~AP_AHRS_View() {}
|
||||||
|
|
||||||
|
@ -52,20 +52,20 @@ public:
|
||||||
float cos_pitch() const {
|
float cos_pitch() const {
|
||||||
return trig.cos_pitch;
|
return trig.cos_pitch;
|
||||||
}
|
}
|
||||||
float cos_yaw() const {
|
float cos_yaw() const {
|
||||||
return trig.cos_yaw;
|
return trig.cos_yaw;
|
||||||
}
|
}
|
||||||
float sin_roll() const {
|
float sin_roll() const {
|
||||||
return trig.sin_roll;
|
return trig.sin_roll;
|
||||||
}
|
}
|
||||||
float sin_pitch() const {
|
float sin_pitch() const {
|
||||||
return trig.sin_pitch;
|
return trig.sin_pitch;
|
||||||
}
|
}
|
||||||
float sin_yaw() const {
|
float sin_yaw() const {
|
||||||
return trig.sin_yaw;
|
return trig.sin_yaw;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
wrappers around ahrs functions which pass-thru directly. See
|
wrappers around ahrs functions which pass-thru directly. See
|
||||||
AP_AHRS.h for description of each function
|
AP_AHRS.h for description of each function
|
||||||
|
@ -73,15 +73,15 @@ public:
|
||||||
bool get_position(struct Location &loc) const {
|
bool get_position(struct Location &loc) const {
|
||||||
return ahrs.get_position(loc);
|
return ahrs.get_position(loc);
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3f wind_estimate(void) {
|
Vector3f wind_estimate(void) {
|
||||||
return ahrs.wind_estimate();
|
return ahrs.wind_estimate();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool airspeed_estimate(float *airspeed_ret) const {
|
bool airspeed_estimate(float *airspeed_ret) const {
|
||||||
return ahrs.airspeed_estimate(airspeed_ret);
|
return ahrs.airspeed_estimate(airspeed_ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool airspeed_estimate_true(float *airspeed_ret) const {
|
bool airspeed_estimate_true(float *airspeed_ret) const {
|
||||||
return ahrs.airspeed_estimate_true(airspeed_ret);
|
return ahrs.airspeed_estimate_true(airspeed_ret);
|
||||||
}
|
}
|
||||||
|
@ -129,21 +129,22 @@ public:
|
||||||
uint32_t getLastPosDownReset(float &posDelta) const {
|
uint32_t getLastPosDownReset(float &posDelta) const {
|
||||||
return ahrs.getLastPosDownReset(posDelta);
|
return ahrs.getLastPosDownReset(posDelta);
|
||||||
}
|
}
|
||||||
|
|
||||||
float roll;
|
float roll;
|
||||||
float pitch;
|
float pitch;
|
||||||
float yaw;
|
float yaw;
|
||||||
int32_t roll_sensor;
|
int32_t roll_sensor;
|
||||||
int32_t pitch_sensor;
|
int32_t pitch_sensor;
|
||||||
int32_t yaw_sensor;
|
int32_t yaw_sensor;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const enum Rotation rotation;
|
const enum Rotation rotation;
|
||||||
AP_AHRS &ahrs;
|
AP_AHRS &ahrs;
|
||||||
|
|
||||||
|
Matrix3f rot_view;
|
||||||
Matrix3f rot_body_to_ned;
|
Matrix3f rot_body_to_ned;
|
||||||
Vector3f gyro;
|
Vector3f gyro;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
float cos_roll;
|
float cos_roll;
|
||||||
float cos_pitch;
|
float cos_pitch;
|
||||||
|
@ -153,4 +154,3 @@ private:
|
||||||
float sin_yaw;
|
float sin_yaw;
|
||||||
} trig;
|
} trig;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue