Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: adapt the DCM_HIL library to AHRS

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-11 19:00:06 +11:00
parent d4bb068d5b
commit 8dcf82b433
2 changed files with 40 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AP_AHRS/AP_AHRS_HIL.cpp
View File

@ -1,53 +1,30 @@
/*
APM_DCM_HIL.cpp - DCM AHRS Library, for Ardupilot Mega, Hardware in the Loop Model
Code by James Goppert. DIYDrones.com
APM_AHRS_HIL.cpp
Hardware in the loop AHRS object
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
Methods:
update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data
get_gyro() : Returns gyro vector corrected for bias
get_accel() : Returns accelerometer vector
get_dcm_matrix() : Returns dcm matrix
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later
version.
*/
#include <AP_DCM_HIL.h>
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
#include <FastSerial.h>
#include <AP_AHRS.h>
/**************************************************/
void
AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega_integ_corr.x = _rollRate;
_omega_integ_corr.y = _pitchRate;
_omega_integ_corr.z = _yawRate;
_omega(_rollRate, _pitchRate, _yawRate);
roll_sensor =ToDeg(roll)*100;
pitch_sensor =ToDeg(pitch)*100;
yaw_sensor =ToDeg(yaw)*100;
// Need the standard C_body<-nav dcm from navigation frame to body frame
// Strapdown Inertial Navigation Technology / Titterton/ pg. 41
float sRoll = sin(roll), cRoll = cos(roll);
float sPitch = sin(pitch), cPitch = cos(pitch);
float sYaw = sin(yaw), cYaw = cos(yaw);
_dcm_matrix.a.x = cPitch*cYaw;
_dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.b.x = cPitch*sYaw;
_dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw;
_dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.c.x = -sPitch;
_dcm_matrix.c.y = sRoll*cPitch;
_dcm_matrix.c.z = cRoll*cPitch;
roll_sensor = ToDeg(roll)*100;
pitch_sensor = ToDeg(pitch)*100;
yaw_sensor = ToDeg(yaw)*100;
}

89
libraries/AP_AHRS/AP_AHRS_HIL.h
View File

@ -1,78 +1,39 @@
#ifndef AP_DCM_HIL_H
#define AP_DCM_HIL_H
#ifndef AP_AHRS_HIL_H
#define AP_AHRS_HIL_H
#include "../FastSerial/FastSerial.h"
#include "../AP_Math/AP_Math.h"
#include <inttypes.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_ADC/AP_ADC.h"
#include "../AP_GPS/AP_GPS.h"
#include "../AP_IMU/AP_IMU.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_DCM_HIL
class AP_AHRS_HIL : public AP_AHRS
{
public:
// Constructors
AP_DCM_HIL() :
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1)
{}
AP_AHRS_HIL(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
}
// Accessors
Vector3f get_gyro(void) {return _omega_integ_corr; }
Vector3f get_accel(void) { return _accel_vector; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
Vector3f get_gyro(void) {return _omega; }
void set_centripetal(bool b) {}
void set_compass(Compass *compass) {}
Matrix3f get_dcm_matrix(void) {
Matrix3f m;
m.from_euler(roll, pitch, yaw);
return m;
}
// Methods
void update_DCM(void) {}
void update_DCM_fast(void) {}
float get_health(void) { return 1.0; }
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100
float roll; // Radians
float pitch; // Radians
float yaw; // Radians
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
float kp_roll_pitch() { return 0; }
void kp_roll_pitch(float v) { }
float ki_roll_pitch() { return 0; }
void ki_roll_pitch(float v) { }
float kp_yaw() { return 0; }
void kp_yaw(float v) { }
float ki_yaw() { return 0; }
void ki_yaw(float v) { }
void update(void) {}
void setHil(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate);
float rollRate, float pitchRate, float yawRate);
// return the current estimate of the gyro drift
Vector3f get_gyro_drift(void) { return Vector3f(0,0,0); }
// reset the current attitude, used on new IMU calibration
void reset(bool recover_eulers=false) {}
float get_error_rp(void) { return 0; }
float get_error_yaw(void) { return 0; }
private:
// Methods
Matrix3f _dcm_matrix;
Vector3f _omega_integ_corr;
Vector3f _accel_vector;
Vector3f _omega;
};
#endif