mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
91 lines
2.6 KiB
C
91 lines
2.6 KiB
C
|
#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 <APM_ADC.h>
|
||
|
|
#include <AP_GPS.h>
|
||
|
|
#include "AP_IMU.h"
|
||
|
|
|
||
|
|
|
||
|
|
class AP_DCM_FW
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
// Constructors
|
||
|
|
AP_DCM_FW(GPS *GPS); // Constructor - no magnetometer
|
||
|
|
AP_DCM_FW(GPS *GPS, APM_Compass_Class *withCompass); // Constructor for case with magnetometer
|
||
|
|
|
||
|
|
// Accessors
|
||
|
|
long get_roll_sensor(void); // Degrees * 100
|
||
|
|
long get_pitch_sensor(void); // Degrees * 100
|
||
|
|
long get_yaw_sensor(void); // Degrees * 100
|
||
|
|
float get_roll(void); // Radians
|
||
|
|
float get_pitch(void); // Radians
|
||
|
|
float get_yaw(void); // Radians
|
||
|
|
|
||
|
|
// Methods
|
||
|
|
void quick_init(void);
|
||
|
|
void init(void);
|
||
|
|
void update_DCM(float _G_Dt);
|
||
|
|
|
||
|
|
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 _gyro_temp_comp(int i, int temp) const;
|
||
|
|
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;
|
||
|
|
|
||
|
|
long roll_sensor; // degrees * 100
|
||
|
|
long pitch_sensor; // degrees * 100
|
||
|
|
long yaw_sensor; // degrees * 100
|
||
|
|
|
||
|
|
float roll; // radians
|
||
|
|
float pitch; // radians
|
||
|
|
float yaw; // radians
|
||
|
|
|
||
|
|
Matrix3f _dcm_matrix;
|
||
|
|
|
||
|
|
float _adc_in[6]; // array that store the 6 ADC channels used by IMU
|
||
|
|
float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers
|
||
|
|
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
|
||
|
|
|
||
|
|
// constants
|
||
|
|
static const uint8_t _sensors[6];
|
||
|
|
static const int _sensor_signs[9];
|
||
|
|
static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature
|
||
|
|
static const float _gyro_temp_curve[3][3];
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif
|
||
|
|
|
||
|
|
|