2012-11-14 12:10:15 -04:00
|
|
|
#ifndef __AP_AHRS_HIL_H__
|
|
|
|
#define __AP_AHRS_HIL_H__
|
2010-12-10 06:17:27 -04:00
|
|
|
|
2012-03-11 05:00:06 -03:00
|
|
|
class AP_AHRS_HIL : public AP_AHRS
|
2010-12-10 06:17:27 -04:00
|
|
|
{
|
|
|
|
public:
|
2012-08-21 23:19:51 -03:00
|
|
|
// Constructors
|
2013-11-03 23:34:32 -04:00
|
|
|
AP_AHRS_HIL(AP_InertialSensor &ins, GPS *&gps) :
|
2013-04-21 09:27:04 -03:00
|
|
|
AP_AHRS(ins, gps),
|
|
|
|
_drift()
|
|
|
|
{}
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// Accessors
|
2013-04-19 04:45:54 -03:00
|
|
|
const Vector3f get_gyro(void) const {
|
2012-08-21 23:19:51 -03:00
|
|
|
return _omega;
|
|
|
|
}
|
|
|
|
|
2013-04-21 09:27:04 -03:00
|
|
|
const Matrix3f &get_dcm_matrix(void) const {
|
|
|
|
return _dcm_matrix;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Methods
|
2012-11-14 12:10:15 -04:00
|
|
|
void update(void) {
|
2013-11-03 23:34:32 -04:00
|
|
|
_ins.update();
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2012-09-15 05:47:18 -03:00
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
void setHil(float roll, float pitch, float yaw,
|
|
|
|
float rollRate, float pitchRate, float yawRate);
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// return the current estimate of the gyro drift
|
2013-04-21 09:27:04 -03:00
|
|
|
const Vector3f &get_gyro_drift(void) const {
|
|
|
|
return _drift;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// reset the current attitude, used on new IMU calibration
|
|
|
|
void reset(bool recover_eulers=false) {
|
|
|
|
}
|
|
|
|
|
2013-11-22 21:37:23 -04:00
|
|
|
void reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) {
|
|
|
|
// not implemented - use setHil()
|
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
float get_error_rp(void) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
float get_error_yaw(void) {
|
|
|
|
return 0;
|
|
|
|
}
|
2011-07-30 17:34:23 -03:00
|
|
|
|
2010-12-10 06:17:27 -04:00
|
|
|
private:
|
2012-08-21 23:19:51 -03:00
|
|
|
Vector3f _omega;
|
2013-04-21 09:27:04 -03:00
|
|
|
Matrix3f _dcm_matrix;
|
|
|
|
Vector3f _drift;
|
2010-12-10 06:17:27 -04:00
|
|
|
};
|
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
#endif // __AP_AHRS_HIL_H__
|