Skip to content
This repository was archived by the owner on Feb 5, 2024. It is now read-only.

Implement sensor fusion calculations involving angles and directions #153

Open
wants to merge 2 commits into
base: devel
Choose a base branch
from
Open

Implement sensor fusion calculations involving angles and directions #153

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b6f3b32
Implement calculations involving angles and directions
antimonyB Sep 21, 2021
3b81714
Subtract gravity from acceleration
antimonyB Oct 12, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions Autopilot/SensorFusion/Inc/MahonyAHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
extern volatile float twoKp; // 2 * proportional gain (Kp)
extern volatile float twoKi; // 2 * integral gain (Ki)
extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
extern volatile float qDiff1, qDiff2, qDiff3, qDiff4; // rate of change of quaternion per SF update

//---------------------------------------------------------------------------------------------------
// Function declarations
Expand Down
9 changes: 5 additions & 4 deletions Autopilot/SensorFusion/Inc/SensorFusion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,17 +26,18 @@ typedef struct {
} SFError_t;

typedef struct {
float roll, pitch, yaw; //rad
float rollRate, pitchRate, yawRate; //rad/s
float roll, pitch, yaw; //Degrees. Yaw of 180 is north.
float rollRate, pitchRate, yawRate; //Degrees/second
float airspeed; //m/s
float altitude; //m
float rateOfClimb; //m/s
long double latitude; //Decimal degrees
float latitudeSpeed; //m/s
long double longitude; //Decimal degrees
float longitudeSpeed; //m/s
double track; // degrees
double heading; //degrees
double track; //Degrees. Track of 0 is north.
float groundSpeed; //m/s
double heading; //Degrees. Heading of 0 is north.
} SFOutput_t;

//Following structs store the raw sensor data so other modules can have direct access to them without including sensor header files
Expand Down
16 changes: 15 additions & 1 deletion Autopilot/SensorFusion/Src/MahonyAHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
volatile float qDiff1 = 0.0f, qDiff2 = 0.0f, qDiff3 = 0.0f, qDiff4 = 0.0f; // rate of change of quaternion per SF update
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki

//---------------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -152,7 +153,7 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
float qa, qb, qc, qd;

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
Expand Down Expand Up @@ -201,6 +202,7 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
qa = q0;
qb = q1;
qc = q2;
qd = q3;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
Expand All @@ -212,6 +214,18 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

float qInvertFactor = 1.0f / (qa*qa + qb*qb + qc*qc + qd*qd);
float qInv1 = qa * qInvertFactor;
float qInv2 = -qb * qInvertFactor;
float qInv3 = -qc * qInvertFactor;
float qInv4 = -qd * qInvertFactor;

//qDiff = qNum * qInv
qDiff1 = (q0 * qInv1 - q1 * qInv2 - q2 * qInv3 - q3 * qInv4);
qDiff2 = (q0 * qInv2 + q1 * qInv1 + q2 * qInv4 - q3 * qInv3);
qDiff3 = (q0 * qInv3 - q1 * qInv4 + q2 * qInv1 + q3 * qInv2);
qDiff4 = (q0 * qInv4 + q1 * qInv3 - q2 * qInv2 + q3 * qInv1);
}

//---------------------------------------------------------------------------------------------------
Expand Down
85 changes: 68 additions & 17 deletions Autopilot/SensorFusion/Src/SensorFusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,11 @@ extern "C"
#endif

typedef struct {
float roll, pitch, yaw; //rad
float rollRate, pitchRate, yawRate; //rad/s
float roll, pitch, yaw; //Degrees. Yaw of 180 is north.
float rollRate, pitchRate, yawRate; //Degrees/second
float airspeed; //m/s
float heading; //Degrees. Heading of 0 is north.
float q0, q1, q2, q3; //Quaternion attitude
} SFAttitudeOutput_t ;

struct SFPathOutput_t{
Expand All @@ -36,6 +38,8 @@ struct SFPathOutput_t{
float latitudeSpeed; //m/s
long double longitude; //Decimal degrees
float longitudeSpeed; //m/s
double track; //Degrees. Track of 0 is north.
float groundSpeed; //m/s
};

const int NUM_KALMAN_VALUES = 6;
Expand Down Expand Up @@ -89,14 +93,36 @@ void SF_Init(void)
iterData.prevP[5] = 0;
}

void localToGlobalAccel(IMUData_t *imudata, float *u)
//Rotates acceleration vector so its direction is no longer relative to the aircrafts's rotation.
//Uses a quaternion-derived rotation matrix:
//wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternions_as_rotations
void localToGlobalAccel(SFAttitudeOutput_t *attitudeData, float uAccel[3])
{
u[0] = imudata->accy; //Vertical acceleration
u[1] = imudata->accx; //Latitudinal acceleration
u[2] = imudata->accz; //Longitudinal acceleration
float q0 = attitudeData->q0;
float q1 = attitudeData->q1;
float q2 = attitudeData->q2;
float q3 = attitudeData->q3;

float rotation[3*3] =
{
1 - 2 * (q2*q2 + q3*q3), 2 * (q1*q2 - q3*q0), 2 * (q1*q3 + q2*q0),
2 * (q1*q2 + q3*q0), 1 - 2 * (q1*q1 + q3*q3), 2 * (q2*q3 - q1*q0),
2 * (q1*q3 - q2*q0), 2 * (q2*q3 + q1*q0), 1 - 2 * (q1*q1 + q2*q2)
};

//Convert axes of u vector to agree with quaternion definition.
//X is north, Y is east, Z is down.
float xyzAccel[3] = {uAccel[1], uAccel[2], -uAccel[0]};
float newAccel[3];
mul(rotation, xyzAccel, newAccel, 3, 3, 1);

//Convert result to original format of u
uAccel[0] = -(xyzAccel[2] - 9.81);
uAccel[1] = xyzAccel[0];
uAccel[2] = xyzAccel[1];
}

//Map gps position to xy coords using the reference location as the origin.
//Map gps position to xy coords using the reference location as the origin
//wikipedia.org/wiki/Geographic_coordinate_system
double * gpsToCartesian(double lat, double lng){
const double RAD_LAT = DEG_TO_RAD(REF_LAT);
Expand Down Expand Up @@ -178,24 +204,34 @@ SFError_t SF_GetAttitude(SFAttitudeOutput_t *Output, IMUData_t *imudata) {
MahonyAHRSupdate(imudata->gyrx, imudata->gyry, imudata->gyrz, imudata->accx, imudata->accy, imudata->accz, imudata->magx, imudata->magy, imudata->magz);

//Convert quaternion output to angles (in deg)
imu_RollAngle = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.29578f;
imu_PitchAngle = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
imu_YawAngle = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29578f + 180.0f;
imu_RollAngle = RAD_TO_DEG(atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2));
imu_PitchAngle = RAD_TO_DEG(asinf(-2.0f * (q1 * q3 - q0 * q2)));
imu_YawAngle = RAD_TO_DEG(atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3)) + 180.0f;


//Convert rate of change of quaternion to angular velocity (in deg/s)
//imu_RollRate = atan2f(qDot1 * qDot2 + qDot3 * qDot4, 0.5f - qDot2 * qDot2 - qDot3 * qDot3) * 57.29578f;
//imu_PitchRate = asinf(-2.0f * (qDot2 * qDot4 - qDot1 * qDot3)) * 57.29578f;
//imu_YawRate = atan2f(qDot2 * qDot3 + qDot1 * qDot4, 0.5f - qDot3 * qDot3 - qDot4 * qDot4) * 57.29578f + 180.0f;
imu_RollRate = RAD_TO_DEG(atan2f(qDiff1 * qDiff2 + qDiff3 * qDiff4, 0.5f - qDiff2 * qDiff2 - qDiff3 * qDiff3)) * SF_FREQ;
imu_PitchRate = RAD_TO_DEG(asinf(-2.0f * (qDiff2 * qDiff4 - qDiff1 * qDiff3))) * SF_FREQ;
imu_YawRate = RAD_TO_DEG(atan2f(qDiff2 * qDiff3 + qDiff1 * qDiff4, 0.5f - qDiff3 * qDiff3 - qDiff4 * qDiff4)) * SF_FREQ;

//Transfer Fused IMU data into SF Output struct
Output->pitch = imu_PitchAngle;
Output->roll = imu_RollAngle;
Output->yaw = imu_YawAngle;

float heading = imu_YawAngle + 180;
if (heading >= 360) heading -= 360;
Output->heading = heading;

Output->pitchRate = imu_PitchRate;
Output->rollRate = imu_RollRate;
Output->yawRate = imu_YawRate;

Output->q0 = q0;
Output->q1 = q1;
Output->q2 = q2;
Output->q3 = q3;

return SFError;
}

Expand Down Expand Up @@ -254,11 +290,12 @@ SFError_t SF_GetPosition(SFPathOutput_t *Output, AltimeterData_t *altimeterdata,
//Measured XYZ acceleration
float u[U_DIM] =
{
0,
0,
0
//TODO: Verify the directions of reported acceleration by the imu
imudata->accy, //Vertical acceleration
imudata->accx, //Latitudinal acceleration
imudata->accz //Longitudinal acceleration
};
localToGlobalAccel(imudata, u);
localToGlobalAccel(attitudedata, u);

//Relationship between distance and acceleration
float ddt = pow(dt,2)/2;
Expand Down Expand Up @@ -446,6 +483,12 @@ SFError_t SF_GetPosition(SFPathOutput_t *Output, AltimeterData_t *altimeterdata,

/*Output*/

float northSpeed = x[3], eastSpeed = x[5];
float track = RAD_TO_DEG(atan2(eastSpeed, northSpeed));
if (track < 0) track += 360;

float groundSpeed = sqrt(northSpeed*northSpeed + eastSpeed*eastSpeed);

double * latLongOut = cartesianToGPS(x[2], x[4]);

Output->altitude = x[0];
Expand All @@ -454,6 +497,8 @@ SFError_t SF_GetPosition(SFPathOutput_t *Output, AltimeterData_t *altimeterdata,
Output->latitudeSpeed = x[3];
Output->longitude = latLongOut[1];
Output->longitudeSpeed = x[5];
Output->track = track;
Output->groundSpeed = groundSpeed;

for (int i = 0; i < DIM*1; i++) iterdata->prevX[i] = newX[i];
for (int i = 0; i < DIM*DIM; i++) iterdata->prevP[i] = newP[i];
Expand Down Expand Up @@ -497,12 +542,18 @@ SFError_t SF_GenerateNewResult()
SFOutput.pitch = attitudeOutput.pitch;
SFOutput.roll = attitudeOutput.roll;
SFOutput.yaw = attitudeOutput.yaw;
SFOutput.pitchRate = attitudeOutput.pitchRate;
SFOutput.rollRate = attitudeOutput.rollRate;
SFOutput.yawRate = attitudeOutput.yawRate;
SFOutput.altitude = pathOutput.altitude;
SFOutput.rateOfClimb = pathOutput.rateOfClimb;
SFOutput.latitude = pathOutput.latitude;
SFOutput.latitudeSpeed = pathOutput.latitudeSpeed;
SFOutput.longitude = pathOutput.longitude;
SFOutput.longitudeSpeed = pathOutput.longitudeSpeed;
SFOutput.track = pathOutput.track;
SFOutput.groundSpeed = pathOutput.groundSpeed;
SFOutput.heading = attitudeOutput.heading;

return SFError;
}
Expand Down