From 94a6debf8e4b2d0e63487b763ef42c514e29ae22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 24 Aug 2023 14:49:29 +1000 Subject: [PATCH] AP_CINS: initial framework for ANU CINS state estimator --- libraries/AP_CINS/AP_CINS.cpp | 136 ++++++++++++++++++++++++++++++++++ libraries/AP_CINS/AP_CINS.h | 60 +++++++++++++++ 2 files changed, 196 insertions(+) create mode 100644 libraries/AP_CINS/AP_CINS.cpp create mode 100644 libraries/AP_CINS/AP_CINS.h diff --git a/libraries/AP_CINS/AP_CINS.cpp b/libraries/AP_CINS/AP_CINS.cpp new file mode 100644 index 00000000000000..3b69e90a444da4 --- /dev/null +++ b/libraries/AP_CINS/AP_CINS.cpp @@ -0,0 +1,136 @@ +/* + CINS state estimator for AP_AHRS + */ + +#include "AP_CINS.h" +#include +#include +#include + +/* + initialise the filter + */ +void AP_CINS::init(void) +{ + state.rotation_matrix.from_euler(0,0,0); +} + +/* + update function, called at loop rate + */ +void AP_CINS::update(void) +{ + auto &dal = AP::dal(); + + // get delta angle + const uint8_t gyro_index = dal.ins().get_primary_gyro(); + Vector3f delta_angle; + float dangle_dt; + if (!dal.ins().get_delta_angle(gyro_index, delta_angle, dangle_dt) || dangle_dt <= 0) { + // can't update, no delta angle + return; + } + // turn delta angle into a gyro in radians/sec + Vector3f gyro = delta_angle / dangle_dt; + + // get delta velocity + Vector3f delta_velocity; + float dvel_dt; + if (!dal.ins().get_delta_velocity(gyro_index, delta_velocity, dvel_dt) || dvel_dt <= 0) { + // can't update, no delta velocity + return; + } + // turn delta velocity into a accel vector + const Vector3f accel = delta_velocity / dvel_dt; + + // see if we have new GPS data + const auto &gps = dal.gps(); + if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) { + const uint32_t last_gps_fix_ms = gps.last_message_time_ms(0); + if (last_gps_update_ms != last_gps_fix_ms) { + last_gps_update_ms = last_gps_fix_ms; + const auto &loc = gps.location(); + if (!state.have_origin) { + state.have_origin = true; + state.origin = loc; + } + const Vector3f pos = state.origin.get_distance_NED(loc); + update_gps(pos); + } + } + + update_imu(gyro, accel, dangle_dt); + + if (state.have_origin) { + // fill in location + state.location = state.origin; + state.location.offset(state.position.x, state.position.y); + state.location.alt -= state.position.z * 100; + } + + // @LoggerMessage: CINS + // @Description: CINS state + // @Field: TimeUS: Time since system startup + // @Field: Roll: euler roll + // @Field: Pitch: euler pitch + // @Field: Yaw: euler yaw + // @Field: VN: velocity north + // @Field: VE: velocity east + // @Field: VD: velocity down + // @Field: PN: position north + // @Field: PE: position east + // @Field: PD: position down + // @Field: Lat: latitude + // @Field: Lon: longitude + // @Field: Alt: altitude AMSL + float 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", + "sdddnnnmmmDUm", + "F000000000GG0", + "QfffffffffLLf", + AP_HAL::micros64(), + degrees(roll_rad), + degrees(pitch_rad), + degrees(yaw_rad), + state.velocity_NED.x, + state.velocity_NED.y, + state.velocity_NED.z, + state.position.x, + state.position.y, + state.position.z, + state.location.lat, + state.location.lng, + state.location.alt*0.01); + + +} + +/* + update on new GPS sample + */ +void AP_CINS::update_gps(const Vector3f &pos) +{ + /* + REPLACE: update position estimate properly + */ + state.position = pos; +} + + +/* + update from IMU data + */ +void AP_CINS::update_imu(const Vector3f &gyro_rads, const Vector3f &accel_mss, const float dt) +{ + // rotate state estimate + state.rotation_matrix.rotate(gyro_rads * dt); + + // rotate accel vector into earth frame + Vector3f accel_ef = state.rotation_matrix * accel_mss; + accel_ef.z += GRAVITY_MSS; + + state.velocity_NED += accel_ef * dt; + state.position += state.velocity_NED * dt; +} diff --git a/libraries/AP_CINS/AP_CINS.h b/libraries/AP_CINS/AP_CINS.h new file mode 100644 index 00000000000000..13b78b82b82f12 --- /dev/null +++ b/libraries/AP_CINS/AP_CINS.h @@ -0,0 +1,60 @@ +/* + CINS state estimator for AP_AHRS + */ + +#pragma once + +#include +#include + +class AP_CINS { +public: + void init(void); + void update(); + + Vector3f get_accel() const { + return state.accel; + } + Vector3f get_gyro() const { + return state.gyro; + } + Quaternion get_quat() const { + Quaternion quat; + quat.from_rotation_matrix(state.rotation_matrix); + return quat; + } + Location get_location() const { + return state.location; + } + Vector3f get_velocity() const { + return state.velocity_NED; + } + bool healthy(void) const { + return state.have_origin; + } + bool get_origin(Location &loc) { + loc = state.origin; + return state.have_origin; + } + +private: + void update_gps(const Vector3f &pos); + void update_imu(const Vector3f &gyro_rads, const Vector3f &accel_mss, const float dt); + + struct { + Vector3f accel; + Vector3f gyro; + Matrix3f rotation_matrix; + Vector3f accel_ef; + Location origin; + bool have_origin; + Location location; + Vector3f velocity_NED; + + // NED position from the origin + Vector3f position; + } state; + + uint32_t last_update_us; + uint32_t last_gps_update_ms; +};