3 changed files with 0 additions and 104 deletions
@ -1,47 +0,0 @@
@@ -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 @@
@@ -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