3 changed files with 0 additions and 104 deletions
@ -1,47 +0,0 @@ |
|||||||
/*
|
|
||||||
* APM_AHRS_HIL.cpp |
|
||||||
* |
|
||||||
* Hardware in the loop AHRS object |
|
||||||
|
|
||||||
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/>.
|
|
||||||
*/ |
|
||||||
|
|
||||||
#include <AP_AHRS.h> |
|
||||||
|
|
||||||
/**************************************************/ |
|
||||||
void |
|
||||||
AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw, |
|
||||||
float _rollRate, float _pitchRate, float _yawRate) |
|
||||||
{ |
|
||||||
roll = _roll; |
|
||||||
pitch = _pitch; |
|
||||||
yaw = _yaw; |
|
||||||
|
|
||||||
_omega(_rollRate, _pitchRate, _yawRate); |
|
||||||
|
|
||||||
roll_sensor = ToDeg(roll)*100; |
|
||||||
pitch_sensor = ToDeg(pitch)*100; |
|
||||||
yaw_sensor = ToDeg(yaw)*100; |
|
||||||
|
|
||||||
_dcm_matrix.from_euler(roll, pitch, yaw); |
|
||||||
|
|
||||||
// update trig values including _cos_roll, cos_pitch
|
|
||||||
update_trig(); |
|
||||||
} |
|
||||||
|
|
||||||
// reset the current attitude, used by HIL
|
|
||||||
void reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) |
|
||||||
{ |
|
||||||
// not implemented - use setHil()
|
|
||||||
} |
|
@ -1,56 +0,0 @@ |
|||||||
#ifndef __AP_AHRS_HIL_H__ |
|
||||||
#define __AP_AHRS_HIL_H__ |
|
||||||
|
|
||||||
class AP_AHRS_HIL : public AP_AHRS |
|
||||||
{ |
|
||||||
public: |
|
||||||
// Constructors
|
|
||||||
AP_AHRS_HIL(AP_InertialSensor &ins, GPS *&gps) :
|
|
||||||
AP_AHRS(ins, gps), |
|
||||||
_drift() |
|
||||||
{} |
|
||||||
|
|
||||||
// Accessors
|
|
||||||
const Vector3f get_gyro(void) const { |
|
||||||
return _omega; |
|
||||||
} |
|
||||||
|
|
||||||
const Matrix3f &get_dcm_matrix(void) const { |
|
||||||
return _dcm_matrix; |
|
||||||
} |
|
||||||
|
|
||||||
// Methods
|
|
||||||
void update(void) { |
|
||||||
_ins.update(); |
|
||||||
} |
|
||||||
|
|
||||||
void setHil(float roll, float pitch, float yaw, |
|
||||||
float rollRate, float pitchRate, float yawRate); |
|
||||||
|
|
||||||
// return the current estimate of the gyro drift
|
|
||||||
const Vector3f &get_gyro_drift(void) const { |
|
||||||
return _drift; |
|
||||||
} |
|
||||||
|
|
||||||
// reset the current attitude, used on new IMU calibration
|
|
||||||
void reset(bool recover_eulers=false) { |
|
||||||
} |
|
||||||
|
|
||||||
void reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) { |
|
||||||
// not implemented - use setHil()
|
|
||||||
} |
|
||||||
|
|
||||||
float get_error_rp(void) { |
|
||||||
return 0; |
|
||||||
} |
|
||||||
float get_error_yaw(void) { |
|
||||||
return 0; |
|
||||||
} |
|
||||||
|
|
||||||
private: |
|
||||||
Vector3f _omega; |
|
||||||
Matrix3f _dcm_matrix; |
|
||||||
Vector3f _drift; |
|
||||||
}; |
|
||||||
|
|
||||||
#endif // __AP_AHRS_HIL_H__
|
|
Loading…
Reference in new issue