Skip to content

Commit

Permalink
AP_CINS: convert to ftype
Browse files Browse the repository at this point in the history
  • Loading branch information
@tridge
tridge committed Sep 16, 2023
1 parent 04c3ffd commit 3a1a748
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 73 deletions.
102 changes: 51 additions & 51 deletions libraries/AP_CINS/AP_CINS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@
void AP_CINS::init(void)
{
//Initialise XHat and ZHat as stationary at the origin
state.XHat = SE23f(Matrix3f(1,0,0,0,1,0,0,0,1),Vector3f(0,0,0),Vector3f(0,0,0),0.0f);
state.ZHat = SE23f(Matrix3f(1,0,0,0,1,0,0,0,1),Vector3f(0,0,0),Vector3f(0,0,0),0.0f);
state.XHat = SE23F(Matrix3F(1,0,0,0,1,0,0,0,1),Vector3F(0,0,0),Vector3F(0,0,0),0.0f);
state.ZHat = SE23F(Matrix3F(1,0,0,0,1,0,0,0,1),Vector3F(0,0,0),Vector3F(0,0,0),0.0f);

state.rotation_matrix.from_euler(radians(CINS_INITIAL_ROLL),radians(CINS_INITIAL_PITCH),radians(CINS_INITIAL_YAW));
state.XHat.init(state.rotation_matrix);
Expand Down Expand Up @@ -60,7 +60,7 @@ void AP_CINS::update(void)
return;
}
// turn delta angle into a gyro in radians/sec
Vector3f gyro = delta_angle / dangle_dt;
Vector3F gyro = (delta_angle / dangle_dt).toftype();

// get delta velocity
Vector3f delta_velocity;
Expand All @@ -70,7 +70,7 @@ void AP_CINS::update(void)
return;
}
// turn delta velocity into a accel vector
const Vector3f accel = delta_velocity / dvel_dt;
const Vector3F accel = (delta_velocity / dvel_dt).toftype();

// see if we have new GPS data
const auto &gps = dal.gps();
Expand All @@ -85,8 +85,8 @@ void AP_CINS::update(void)
state.have_origin = true;
state.origin = loc;
}
const Vector3f pos = state.origin.get_distance_NED(loc);
update_gps(pos, gps_dt);
const Vector3d pos = state.origin.get_distance_NED_double(loc);
update_gps(pos.toftype(), gps_dt);
}
}

Expand Down Expand Up @@ -116,7 +116,7 @@ void AP_CINS::update(void)
// @Field: Lat: latitude
// @Field: Lon: longitude
// @Field: Alt: altitude AMSL
float roll_rad, pitch_rad, yaw_rad;
ftype roll_rad, pitch_rad, yaw_rad;
state.rotation_matrix.to_euler(&roll_rad, &pitch_rad, &yaw_rad);

AP::logger().WriteStreaming("CINS", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,Lat,Lon,Alt",
Expand Down Expand Up @@ -145,7 +145,7 @@ void AP_CINS::update(void)
/*
update on new GPS sample
*/
void AP_CINS::update_gps(const Vector3f &pos, const float gps_dt)
void AP_CINS::update_gps(const Vector3F &pos, const ftype gps_dt)
{
//Jump set update
//compute correction terms
Expand All @@ -161,20 +161,20 @@ void AP_CINS::update_gps(const Vector3f &pos, const float gps_dt)
state.velocity_NED = state.XHat.w();

// use AHRS3 for debugging
float roll_rad, pitch_rad, yaw_rad;
const auto rot = state.rotation_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
ftype roll_rad, pitch_rad, yaw_rad;
const auto rot = state.rotation_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body().toftype();
rot.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
const mavlink_ahrs3_t pkt {
roll : roll_rad,
pitch : pitch_rad,
yaw : yaw_rad,
altitude : -state.position.z,
lat: state.location.lat,
lng: state.location.lng,
v1 : 0,
v2 : 0,
v3 : 0,
v4 : 0
roll : float(roll_rad),
pitch : float(pitch_rad),
yaw : float(yaw_rad),
altitude : float(-state.position.z),
lat: state.location.lat,
lng: state.location.lng,
v1 : 0,
v2 : 0,
v3 : 0,
v4 : 0
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_AHRS3, (const char *)&pkt);
}
Expand All @@ -183,17 +183,17 @@ void AP_CINS::update_gps(const Vector3f &pos, const float gps_dt)
/*
update from IMU data
*/
void AP_CINS::update_imu(const Vector3f &gyro_rads, const Vector3f &accel_mss, const float dt)
void AP_CINS::update_imu(const Vector3F &gyro_rads, const Vector3F &accel_mss, const ftype dt)
{
/*
Normal time update, based of the CINS Disc alg developed by Dr Pieter Van goor and Pat Wiltshire
*/
//Integrate Dynamics using the Matrix exponential
//Create Zero vector
Vector3f zero_vector;
Vector3F zero_vector;
zero_vector.zero();
Vector3f gravity_vector;
gravity_vector = Vector3f(0,0,GRAVITY_MSS);
Vector3F gravity_vector;
gravity_vector = Vector3F(0,0,GRAVITY_MSS);

//Update XHat (Observer Dynamics)
state.XHat = (SE23::exponential(zero_vector, zero_vector, gravity_vector*dt, -dt)*state.XHat) * SE23::exponential(gyro_rads*dt, zero_vector, accel_mss*dt, dt);
Expand All @@ -213,48 +213,48 @@ Delta and Gamma which is then applied to the dynamics. This allows the update_gp
velocity_NED and position.
*/

void AP_CINS::update_correction_terms(const Vector3f &pos, const float gps_dt)
void AP_CINS::update_correction_terms(const Vector3F &pos, const ftype gps_dt)
{
//Collect position estimate from XHat and ZHat
Vector3f pos_est = state.XHat.x();
Vector3f pos_z = state.ZHat.x();
Vector3F pos_est = state.XHat.x();
Vector3F pos_z = state.ZHat.x();
//Create Components of correction terms
Vector3f pos_tz = pos - pos_z;
Vector3f pos_ez = pos_est - pos_z;
Vector3f pos_te = pos - pos_est;
Vector3F pos_tz = pos - pos_z;
Vector3F pos_ez = pos_est - pos_z;
Vector3F pos_te = pos - pos_est;

//Gamma
Matrix3f R_gamma;
Matrix3F R_gamma;
R_gamma.identity();
Vector3f V_gamma_1 = pos_tz * correction_terms.gain_l1*gps_dt;
Vector3f V_gamma_2 = pos_tz * correction_terms.gain_l2*gps_dt;
Vector3F V_gamma_1 = pos_tz * correction_terms.gain_l1*gps_dt;
Vector3F V_gamma_2 = pos_tz * correction_terms.gain_l2*gps_dt;
float alpha_gamma = -1*state.ZHat.alpha();
correction_terms.Gamma = SE23f(R_gamma, V_gamma_1, V_gamma_2, alpha_gamma);
correction_terms.Gamma = SE23F(R_gamma, V_gamma_1, V_gamma_2, alpha_gamma);

//Delta
//R_delta is incorrect, why? TODO
Matrix3f R_delta;
Vector3f maucross_mauhat = Matrix3f::skew_symmetric(pos_te) * pos_ez;
Matrix3F R_delta;
Vector3F maucross_mauhat = Matrix3F::skew_symmetric(pos_te) * pos_ez;
float mautrans_mauhat = pos_tz * pos_ez; //Dotproduct
float norm_maucross_mauhat = maucross_mauhat.length();
//Check is not too small as would lead to too large of a correction, if so dont apply correction
if (fabsF(norm_maucross_mauhat) > 0.00001f){
float psi = atan2F(norm_maucross_mauhat, mautrans_mauhat);
R_delta = Matrix3f::from_angular_velocity(maucross_mauhat * ((correction_terms.gain_p*psi*gps_dt )/norm_maucross_mauhat));
R_delta = Matrix3F::from_angular_velocity(maucross_mauhat * ((correction_terms.gain_p*psi*gps_dt )/norm_maucross_mauhat));
}
else {
R_delta.identity();
}
//Create V_delta
//prelimaries
Vector3f Q = R_delta * pos_ez;
Vector3f Q_1 = Q * correction_terms.gain_l1*gps_dt;
Vector3f Q_2 = Q * correction_terms.gain_l2*gps_dt;
Vector3F Q = R_delta * pos_ez;
Vector3F Q_1 = Q * correction_terms.gain_l1*gps_dt;
Vector3F Q_2 = Q * correction_terms.gain_l2*gps_dt;
//Calculate components of V_delta
Vector3f V_delta_1 = V_gamma_1 - Q_1 -(V_gamma_2 - Q_2)*alpha_gamma;
Vector3f V_delta_2 = V_gamma_2 - Q_2;
Vector3F V_delta_1 = V_gamma_1 - Q_1 -(V_gamma_2 - Q_2)*alpha_gamma;
Vector3F V_delta_2 = V_gamma_2 - Q_2;
//Assign Delta
SE23f Delta = SE23f(R_delta, V_delta_1, V_delta_2, 0.0f);
SE23F Delta = SE23F(R_delta, V_delta_1, V_delta_2, 0.0f);
//Rebase delta
correction_terms.Delta = (state.ZHat * Delta) * SE23::inverse_ZHat(state.ZHat);
}
Expand All @@ -264,11 +264,11 @@ void AP_CINS::update_correction_terms(const Vector3f &pos, const float gps_dt)
*/
bool AP_CINS::init_yaw(void)
{
float mag_yaw, dt;
ftype mag_yaw, dt;
if (!get_compass_yaw(mag_yaw, dt)) {
return false;
}
float roll_rad, pitch_rad, yaw_rad;
ftype roll_rad, pitch_rad, yaw_rad;
state.rotation_matrix.to_euler(&roll_rad, &pitch_rad, &yaw_rad);

state.rotation_matrix.from_euler(roll_rad,pitch_rad,mag_yaw);
Expand All @@ -280,7 +280,7 @@ bool AP_CINS::init_yaw(void)
/*
get yaw from compass
*/
bool AP_CINS::get_compass_yaw(float &yaw_rad, float &dt)
bool AP_CINS::get_compass_yaw(ftype &yaw_rad, ftype &dt)
{
auto &dal = AP::dal();
const auto &compass = dal.compass();
Expand Down Expand Up @@ -326,16 +326,16 @@ bool AP_CINS::get_compass_yaw(float &yaw_rad, float &dt)
*/
void AP_CINS::update_yaw_from_compass(void)
{
float mag_yaw, dt;
ftype mag_yaw, dt;
if (!get_compass_yaw(mag_yaw, dt)) {
return;
}
float roll_rad, pitch_rad, yaw_rad;
ftype roll_rad, pitch_rad, yaw_rad;
state.rotation_matrix.to_euler(&roll_rad, &pitch_rad, &yaw_rad);

const float yaw_err = wrap_PI(mag_yaw - yaw_rad);
const Vector3f yaw_rot_ef{0,0,yaw_err*dt*CINS_MAG_GAIN};
const Vector3f yaw_rot_bf = state.rotation_matrix.mul_transpose(yaw_rot_ef);
const ftype yaw_err = wrap_PI(mag_yaw - yaw_rad);
const Vector3F yaw_rot_ef{0,0,yaw_err*dt*CINS_MAG_GAIN};
const Vector3F yaw_rot_bf = state.rotation_matrix.mul_transpose(yaw_rot_ef);

state.rotation_matrix.rotate(yaw_rot_bf);
state.XHat.init(state.rotation_matrix);
Expand Down
45 changes: 23 additions & 22 deletions libraries/AP_CINS/AP_CINS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#pragma once

#include <AP_Math/AP_Math.h>
#include <AP_Math/SE23.h>
#include <AP_Common/Location.h>

class AP_CINS {
Expand All @@ -13,21 +14,21 @@ class AP_CINS {
void update();

Vector3f get_accel() const {
return state.accel;
return state.accel.tofloat();
}
Vector3f get_gyro() const {
return state.gyro;
return state.gyro.tofloat();
}
Quaternion get_quat() const {
Quaternion quat;
QuaternionF quat;
quat.from_rotation_matrix(state.rotation_matrix);
return quat;
return quat.tofloat();
}
Location get_location() const {
return state.location;
}
Vector3f get_velocity() const {
return state.velocity_NED;
return state.velocity_NED.tofloat();
}
bool healthy(void) const {
return state.have_origin;
Expand All @@ -38,36 +39,36 @@ class AP_CINS {
}

private:
void update_gps(const Vector3f &pos, const float gps_dt);
void update_imu(const Vector3f &gyro_rads, const Vector3f &accel_mss, const float dt);
void update_correction_terms(const Vector3f &pos, const float dt);
void update_gps(const Vector3F &pos, const ftype gps_dt);
void update_imu(const Vector3F &gyro_rads, const Vector3F &accel_mss, const ftype dt);
void update_correction_terms(const Vector3F &pos, const ftype dt);
bool init_yaw(void);
bool get_compass_yaw(float &yaw_rad, float &dt);
bool get_compass_yaw(ftype &yaw_rad, ftype &dt);
void update_yaw_from_compass();

struct {
Vector3f accel;
Vector3f gyro;
Matrix3f rotation_matrix;
Vector3f accel_ef;
Vector3F accel;
Vector3F gyro;
Matrix3F rotation_matrix;
Vector3F accel_ef;
Location origin;
bool have_origin;
Location location;
Vector3f velocity_NED;
Vector3F velocity_NED;
//XHat and ZHat for CINS
SE23f XHat;
SE23f ZHat;
SE23F XHat;
SE23F ZHat;
// NED position from the origin
Vector3f position;
Vector3F position;
} state;
//Struct for correction terms
struct{
SE23f Gamma;
SE23f Delta;
SE23F Gamma;
SE23F Delta;
//Gains for correction terms
float gain_p;
float gain_l1;
float gain_l2;
ftype gain_p;
ftype gain_l1;
ftype gain_l2;
} correction_terms;

uint32_t last_gps_update_ms;
Expand Down

0 comments on commit 3a1a748

Please sign in to comment.