You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
79 lines
2.1 KiB
79 lines
2.1 KiB
#ifndef AP_DCM_FW_h |
|
#define AP_DCM_FW_h |
|
|
|
#include <FastSerial.h> |
|
#include <AP_Math.h> |
|
#include <inttypes.h> |
|
#include "WProgram.h" |
|
#include <APM_Compass.h> |
|
#include <AP_ADC.h> |
|
#include <AP_GPS.h> |
|
#include <AP_IMU.h> |
|
|
|
|
|
class AP_DCM_FW |
|
{ |
|
public: |
|
// Constructors |
|
AP_DCM_FW(AP_ADC * adc, GPS *gps); // Constructor - no magnetometer |
|
AP_DCM_FW(AP_ADC * adc, GPS *gps, APM_Compass_Class *withCompass); // Constructor for case with magnetometer |
|
|
|
// Accessors |
|
Vector3f get_gyros(void); |
|
Vector3f get_accels(void); |
|
Matrix3f get_dcm_matrix(void); |
|
AP_IMU * get_imu(void); |
|
|
|
// Methods |
|
void quick_init(uint16_t *_offset_address); |
|
void gyro_init(uint16_t *_offset_address); |
|
void init(uint16_t *_offset_address); |
|
void update_DCM(float _G_Dt); |
|
|
|
long roll_sensor; // Degrees * 100 |
|
long pitch_sensor; // Degrees * 100 |
|
long yaw_sensor; // Degrees * 100 |
|
float roll; // Radians |
|
float pitch; // Radians |
|
float yaw; // Radians |
|
float imu_health; //Metric based on accel gain deweighting |
|
uint8_t gyro_sat_count; |
|
uint8_t adc_constraints; |
|
uint8_t renorm_sqrt_count; |
|
uint8_t renorm_blowup_count; |
|
|
|
private: |
|
// Methods |
|
void read_adc_raw(void); |
|
void accel_adjust(void); |
|
float read_adc(int select); |
|
void matrix_update(float _G_Dt); |
|
void normalize(void); |
|
Vector3f renorm(Vector3f const &a, int &problem); |
|
void drift_correction(void); |
|
void euler_angles(void); |
|
|
|
// members |
|
APM_Compass_Class *_compass; |
|
GPS *_gps; |
|
AP_IMU _imu; |
|
|
|
Matrix3f _dcm_matrix; |
|
|
|
Vector3f _accel_vector; // Store the acceleration in a vector |
|
Vector3f _gyro_vector; //Store the gyros turn rate in a vector |
|
Vector3f _omega_P; //Omega Proportional correction |
|
Vector3f _omega_I; //Omega Integrator correction |
|
Vector3f _omega_integ_corr; //Partially corrected Gyro_Vector data - used for centrepetal correction |
|
Vector3f _omega; //Corrected Gyro_Vector data |
|
Vector3f _error_roll_pitch; |
|
Vector3f _error_yaw; |
|
float _errorCourse; |
|
float _course_over_ground_x; //Course overground X axis |
|
float _course_over_ground_y; //Course overground Y axis |
|
|
|
}; |
|
|
|
#endif |
|
|
|
|
|
|