diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 8332f828e4..c0f5dfc1be 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ class AC_AttitudeControl { public: - AC_AttitudeControl( AP_AHRS &ahrs, + AC_AttitudeControl( AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_Motors& motors, float dt) : @@ -369,7 +369,7 @@ protected: float _throttle_rpy_mix; // References to external libraries - const AP_AHRS& _ahrs; + const AP_AHRS_View& _ahrs; const AP_Vehicle::MultiCopter &_aparm; AP_Motors& _motors; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 4e1eb72af2..a7cf11146b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -30,7 +30,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { public: - AC_AttitudeControl_Heli( AP_AHRS &ahrs, + AC_AttitudeControl_Heli( AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsHeli& motors, float dt) : diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 13f1337d26..0241ebc4e9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -145,7 +145,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { AP_GROUPEND }; -AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) : +AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) : AC_AttitudeControl(ahrs, aparm, motors, dt), _motors_multi(motors), _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 3935cfac01..349c8da22b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -41,7 +41,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { public: - AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt); + AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt); // empty destructor to suppress compiler warning virtual ~AC_AttitudeControl_Multi() {} diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 1864a14609..46fcc013d0 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -23,7 +23,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, +AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, const AP_Motors& motors, AC_AttitudeControl& attitude_control, AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z, AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy) : diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index ac7faeff23..e2fcc44ca9 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -47,7 +47,7 @@ class AC_PosControl public: /// Constructor - AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, + AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav, const AP_Motors& motors, AC_AttitudeControl& attitude_control, AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z, AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy); @@ -366,7 +366,7 @@ private: void check_for_ekf_z_reset(); // references to inertial nav and ahrs libraries - const AP_AHRS& _ahrs; + const AP_AHRS_View & _ahrs; const AP_InertialNav& _inav; const AP_Motors& _motors; AC_AttitudeControl& _attitude_control; diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp new file mode 100644 index 0000000000..04e7502ec8 --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -0,0 +1,45 @@ +/* + 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 . + */ + +/* + * AHRS View class - for creating a 2nd view of the vehicle attitude + * + */ + +#include "AP_AHRS_View.h" + +AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) : + rotation(_rotation), + ahrs(_ahrs) +{ + +} + +// update state +void AP_AHRS_View::update(void) +{ + rot_body_to_ned = ahrs.get_rotation_body_to_ned(); + gyro = ahrs.get_gyro(); + + rot_body_to_ned.to_euler(&roll, &pitch, &yaw); + + roll_sensor = degrees(roll) * 100; + pitch_sensor = degrees(pitch) * 100; + yaw_sensor = degrees(yaw) * 100; + + ahrs.calc_trig(rot_body_to_ned, + trig.cos_roll, trig.cos_pitch, trig.cos_yaw, + trig.sin_roll, trig.sin_pitch, trig.sin_yaw); +} diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h new file mode 100644 index 0000000000..a8382e1a4e --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -0,0 +1,156 @@ +#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 . + */ + +/* + * AHRS View class - for creating a 2nd view of the vehicle attitude + * + */ + +#include "AP_AHRS.h" + +class AP_AHRS_View +{ +public: + // Constructor + AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation); + + // update state + void update(void); + + // empty virtual destructor + virtual ~AP_AHRS_View() {} + + // return a smoothed and corrected gyro vector + const Vector3f &get_gyro(void) const { + return gyro; + } + + // 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; + } + float cos_yaw() const { + return trig.cos_yaw; + } + float sin_roll() const { + return trig.sin_roll; + } + float sin_pitch() const { + return trig.sin_pitch; + } + float sin_yaw() const { + return trig.sin_yaw; + } + + + /* + 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); + } + + Vector3f wind_estimate(void) { + return ahrs.wind_estimate(); + } + + bool airspeed_estimate(float *airspeed_ret) const { + return ahrs.airspeed_estimate(airspeed_ret); + } + + 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); + } + + bool get_relative_position_NED(Vector3f &vec) const { + return ahrs.get_relative_position_NED(vec); + } + + bool get_relative_position_NE(Vector2f &vecNE) const { + return ahrs.get_relative_position_NE(vecNE); + } + + bool get_relative_position_D(float &posD) const { + return get_relative_position_D(posD); + } + + 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); + } + + float roll; + float pitch; + float yaw; + int32_t roll_sensor; + int32_t pitch_sensor; + int32_t yaw_sensor; + +private: + const enum Rotation rotation; + AP_AHRS &ahrs; + + Matrix3f rot_body_to_ned; + Vector3f gyro; + + struct { + float cos_roll; + float cos_pitch; + float cos_yaw; + float sin_roll; + float sin_pitch; + float sin_yaw; + } trig; +}; +