2017-02-11 04:14:23 -04:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
* AHRS View class - for creating a 2nd view of the vehicle attitude
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "AP_AHRS.h"
|
|
|
|
|
|
|
|
class AP_AHRS_View
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructor
|
2018-11-12 00:58:54 -04:00
|
|
|
AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);
|
2017-02-11 04:14:23 -04:00
|
|
|
|
|
|
|
// update state
|
2017-04-02 22:03:45 -03:00
|
|
|
void update(bool skip_ins_update=false);
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
// empty virtual destructor
|
|
|
|
virtual ~AP_AHRS_View() {}
|
|
|
|
|
|
|
|
// return a smoothed and corrected gyro vector
|
|
|
|
const Vector3f &get_gyro(void) const {
|
|
|
|
return gyro;
|
|
|
|
}
|
|
|
|
|
2017-03-02 07:33:13 -04:00
|
|
|
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
|
|
|
|
Vector3f get_gyro_latest(void) const;
|
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
// return a DCM rotation matrix representing our current
|
|
|
|
// attitude in this view
|
|
|
|
const Matrix3f &get_rotation_body_to_ned(void) const {
|
|
|
|
return rot_body_to_ned;
|
|
|
|
}
|
|
|
|
|
|
|
|
// helper trig value accessors
|
|
|
|
float cos_roll() const {
|
|
|
|
return trig.cos_roll;
|
|
|
|
}
|
|
|
|
float cos_pitch() const {
|
|
|
|
return trig.cos_pitch;
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
float cos_yaw() const {
|
2017-02-11 04:14:23 -04:00
|
|
|
return trig.cos_yaw;
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
float sin_roll() const {
|
2017-02-11 04:14:23 -04:00
|
|
|
return trig.sin_roll;
|
|
|
|
}
|
|
|
|
float sin_pitch() const {
|
|
|
|
return trig.sin_pitch;
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
float sin_yaw() const {
|
2017-02-11 04:14:23 -04:00
|
|
|
return trig.sin_yaw;
|
|
|
|
}
|
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
/*
|
|
|
|
wrappers around ahrs functions which pass-thru directly. See
|
|
|
|
AP_AHRS.h for description of each function
|
|
|
|
*/
|
|
|
|
bool get_position(struct Location &loc) const {
|
|
|
|
return ahrs.get_position(loc);
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
Vector3f wind_estimate(void) {
|
|
|
|
return ahrs.wind_estimate();
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
bool airspeed_estimate(float *airspeed_ret) const {
|
|
|
|
return ahrs.airspeed_estimate(airspeed_ret);
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
bool airspeed_estimate_true(float *airspeed_ret) const {
|
|
|
|
return ahrs.airspeed_estimate_true(airspeed_ret);
|
|
|
|
}
|
|
|
|
|
|
|
|
float get_EAS2TAS(void) const {
|
|
|
|
return ahrs.get_EAS2TAS();
|
|
|
|
}
|
|
|
|
|
|
|
|
Vector2f groundspeed_vector(void) {
|
|
|
|
return ahrs.groundspeed_vector();
|
|
|
|
}
|
|
|
|
|
|
|
|
bool get_velocity_NED(Vector3f &vec) const {
|
|
|
|
return ahrs.get_velocity_NED(vec);
|
|
|
|
}
|
|
|
|
|
|
|
|
bool get_expected_mag_field_NED(Vector3f &ret) const {
|
|
|
|
return ahrs.get_expected_mag_field_NED(ret);
|
|
|
|
}
|
|
|
|
|
2017-02-12 21:02:33 -04:00
|
|
|
bool get_relative_position_NED_home(Vector3f &vec) const {
|
|
|
|
return ahrs.get_relative_position_NED_home(vec);
|
2017-02-11 04:14:23 -04:00
|
|
|
}
|
|
|
|
|
2017-02-12 21:02:33 -04:00
|
|
|
bool get_relative_position_NED_origin(Vector3f &vec) const {
|
|
|
|
return ahrs.get_relative_position_NED_origin(vec);
|
|
|
|
}
|
|
|
|
|
|
|
|
bool get_relative_position_NE_home(Vector2f &vecNE) const {
|
|
|
|
return ahrs.get_relative_position_NE_home(vecNE);
|
2017-02-11 04:14:23 -04:00
|
|
|
}
|
|
|
|
|
2017-02-12 21:02:33 -04:00
|
|
|
bool get_relative_position_NE_origin(Vector2f &vecNE) const {
|
|
|
|
return ahrs.get_relative_position_NE_origin(vecNE);
|
|
|
|
}
|
|
|
|
|
2017-02-19 23:31:53 -04:00
|
|
|
void get_relative_position_D_home(float &posD) const {
|
|
|
|
ahrs.get_relative_position_D_home(posD);
|
2017-02-11 04:14:23 -04:00
|
|
|
}
|
|
|
|
|
2017-02-12 21:02:33 -04:00
|
|
|
bool get_relative_position_D_origin(float &posD) const {
|
2017-02-19 23:31:53 -04:00
|
|
|
return ahrs.get_relative_position_D_origin(posD);
|
2017-02-12 21:02:33 -04:00
|
|
|
}
|
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
float groundspeed(void) {
|
|
|
|
return ahrs.groundspeed();
|
|
|
|
}
|
|
|
|
|
|
|
|
const Vector3f &get_accel_ef_blended(void) const {
|
|
|
|
return ahrs.get_accel_ef_blended();
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t getLastPosNorthEastReset(Vector2f &pos) const {
|
|
|
|
return ahrs.getLastPosNorthEastReset(pos);
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t getLastPosDownReset(float &posDelta) const {
|
|
|
|
return ahrs.getLastPosDownReset(posDelta);
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-09-04 03:53:56 -03:00
|
|
|
// rotate a 2D vector from earth frame to body frame
|
|
|
|
// in result, x is forward, y is right
|
|
|
|
Vector2f rotate_earth_to_body2D(const Vector2f &ef_vector) const;
|
|
|
|
|
|
|
|
// rotate a 2D vector from earth frame to body frame
|
|
|
|
// in input, x is forward, y is right
|
|
|
|
Vector2f rotate_body_to_earth2D(const Vector2f &bf) const;
|
|
|
|
|
2017-03-30 20:32:33 -03:00
|
|
|
// return the average size of the roll/pitch error estimate
|
|
|
|
// since last call
|
|
|
|
float get_error_rp(void) const {
|
|
|
|
return ahrs.get_error_rp();
|
|
|
|
}
|
|
|
|
|
|
|
|
// return the average size of the yaw error estimate
|
|
|
|
// since last call
|
|
|
|
float get_error_yaw(void) const {
|
|
|
|
return ahrs.get_error_yaw();
|
|
|
|
}
|
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
float roll;
|
|
|
|
float pitch;
|
|
|
|
float yaw;
|
|
|
|
int32_t roll_sensor;
|
|
|
|
int32_t pitch_sensor;
|
|
|
|
int32_t yaw_sensor;
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
private:
|
|
|
|
const enum Rotation rotation;
|
|
|
|
AP_AHRS &ahrs;
|
2017-02-11 04:15:34 -04:00
|
|
|
|
|
|
|
Matrix3f rot_view;
|
2017-02-11 04:14:23 -04:00
|
|
|
Matrix3f rot_body_to_ned;
|
|
|
|
Vector3f gyro;
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2017-02-11 04:14:23 -04:00
|
|
|
struct {
|
|
|
|
float cos_roll;
|
|
|
|
float cos_pitch;
|
|
|
|
float cos_yaw;
|
|
|
|
float sin_roll;
|
|
|
|
float sin_pitch;
|
|
|
|
float sin_yaw;
|
|
|
|
} trig;
|
2018-09-25 12:14:36 -03:00
|
|
|
|
|
|
|
float y_angle;
|
2017-02-11 04:14:23 -04:00
|
|
|
};
|