/*
* sparkfun_imu_ahrs.c
*
*
* Created by Filipe Varela on 06/04/20.
* Copyright 2006 Filipe Varela. All rights reserved.
*
* All code custom writen except:
* Kalman algorythm for state_update and kalman_update from rotomotion's tilt.c, credit included in file
* Cross axis interference processing matrix from "The Global Positioning System and Inertial Navigation" ISBN: XXX XXXXXXX
*
* As a sidenote, accel2angle scales ADC to G's for future convenience. It is not required for the current status of the project
* because trig functions only need equally scaled values. The units are irrelevant.
*
*/
#define a2v 00.0048828125
#define r2d 57.29577951308
#define d2r 00.01745329252
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
//socket to broadcast attitude data to another application
struct sockaddr_in sockaddress;
int my_socket;
typedef struct{
float time;
long satelites;
float latitude;
float longitude;
float altitude;
} gpsPacketStruct;
unsigned char ultrasound_raw;
float ultrasound;
unsigned char compass_low,compass_high;
float compass_heading;
gpsPacketStruct gpsPacket;
//state variables
float corrected_rates[3]; //p,q,r in rad/sec
float accels[3]; //Ax,Ay,Az
float gyro_25v_correction[3]; //correction factor for null offset
//kalman estimates
float angle_reference_from_accels[3] = {0.0,0.0,0.0}; //accelerometer based angle estimate
float old_angle_reference_from_accels[3] = {0.0,0.0,0.0}; //previous accelerometer based angle estimate
float angle_estimate[3]; //pretty obvious (current state est)
float gyro_bias[3]={0.0,0.0,0.0};
//raw adc temperature and null temperature
float raw_gyro_temperatures[3]; //roll,pitch,yaw
float raw_gyro_temperatures_null[3] = {524.0,526.0,526.0}; //null values at 25deg (nominal)
//raw adc rate and null rate
float raw_gyro_rates[3]; //roll,pitch,yaw
float raw_gyro_rates_null[3] = {480.0,512.0,496.0}; //rateout at idle (not spinning)
//raw adc gyro 2.5volt reference outputs (for tension (voltage) variation estimate)
float raw_gyro_25v[3]; //roll, pitch, yaw
//raw adc accelerometers
float raw_accels[3]; //x,y,z
float raw_accels_null[3] = {504,504.0,504.0}; //x,y,z at rest and flat
//battery status
float battery;
//gps
float gps_heading;
//period estimation (POSIX time.h) change when resident in atmega
struct timeval start,end;
double dt = 0;
//unix serial port
FILE *fp;
//
float P[3][2][2] = {
{
{ 1, 0 },
{ 0, 1 },
},
{
{ 1, 0 },
{ 0, 1 },
},
{
{ 1, 0 },
{ 0, 1 },
}
};
//accelerometer measurement covariance noise
const float R_angle = 0.3;
const float Q_angle = 0.001;
const float Q_gyro = 0.003;
//accelerometer bias estimate (updated in kalman_update)
float gyro_bias[3];
//prototypes
int readimu(void);
int checkvars(void);
void compensate_gyro_temp_drifts(void);
void accel2angle(void);
void gyro2angle(void);
void state_update();
void kalman_update(int axis_index);
double timeval_subtract(struct timeval *x, struct timeval *y);
/******************************************************************************
MAIN LOOP
BROADCASTS UDP FOR THE COCOA HUD/ARTIFICIAL HORIZON (float phi,theta,psi)
*******************************************************************************/
int main(int argc, char *argv[]){
char startstop[2] = {'A','Z'};
int i;
//inicializar rotinas internas (uarts, timers, etc)
fp = fopen("/dev/cu.USB Serial","r");
if(!fp){
printf("Could not open serial port\n");
return 0;
}
//clean possible buffer contents
for(i=0;i<100;i++)
readimu();
my_socket = socket(AF_INET, SOCK_DGRAM, 0);
sockaddress.sin_family = AF_INET;
sockaddress.sin_port = htons(9999); //host to network short int
sockaddress.sin_addr.s_addr = INADDR_BROADCAST;
int yes=1;
setsockopt(my_socket, SOL_SOCKET, SO_BROADCAST, (char *)&yes, sizeof(yes));
//loop principal
while(1){
gettimeofday(&start,0);
if(readimu()){
gettimeofday(&end,0);
dt = timeval_subtract(&end,&start);
compensate_gyro_temp_drifts();
gyro2angle();
state_update();
accel2angle();
//check if this accel value is different from last
for(i=0;i<3;i++){
//got new accel angle estimate?
//if(old_angle_reference_from_accels[i] != angle_reference_from_accels[i]){
//update kalman gain
kalman_update(i);
}
//regardless, update old value to this value
//old_angle_reference_from_accels[i] = angle_reference_from_accels[i];
}
//put roll pitch yaw on the wire
sendto(my_socket, &startstop[0], 1, 0, (struct sockaddr *)&sockaddress, sizeof(sockaddress));
sendto(my_socket, &angle_estimate, sizeof(angle_estimate), 0, (struct sockaddr *)&sockaddress, sizeof(sockaddress));
sendto(my_socket, &startstop[1], 1, 0, (struct sockaddr *)&sockaddress, sizeof(sockaddress));
}
else
printf("Read imu returned false\n");
}
close(my_socket);
return 0;
}
/******************************************************************************
ESTIMATE ATTITUDE WITH A KF. NOT REALLY IMPLEMENTED YET, BUT THE
NAME SUGGESTS WHAT THE INTENTION IS.
*******************************************************************************/
void gyro2angle(void){
int i;
float gyro_components[3]; // dot values from matrix
float sin_roll,tan_pitch, cos_roll, cos_pitch;
//values were in euler. trig could never work right. try matrix again now
//that we got our internals 100% radians
sin_roll = sin(angle_estimate[0]);
cos_roll = cos(angle_estimate[0]);
cos_pitch = cos(angle_estimate[1]);
tan_pitch = tan(angle_estimate[1]);
/***************************************************************************
ESTIMATOR MATRIX
---------------------------------------
[phi_dot ] [1 sin(phi)tan(theta) cos(phi)*tan(theta) ] [P]
[theta_dot ] = [0 cos(phi) -sin(phi) ] ] * [Q]
[psi_dot ] [0 sin(phi)/cos(theta) cos(phi)/cos(theta) ] [R]
***************************************************************************/
//angle-estimate from matrix
gyro_components[0] = corrected_rates[0]*1 + corrected_rates[1]*sin_roll*tan_pitch + corrected_rates[2]*cos_roll*tan_pitch;
gyro_components[1] = corrected_rates[0]*0 + corrected_rates[1]*cos_roll + corrected_rates[2]*-sin_roll;
gyro_components[2] = corrected_rates[0]*0 + corrected_rates[1]*(sin_roll/cos_pitch) + corrected_rates[2]*(cos_roll*cos_pitch);
for(i=0;i<3;i++){
//state estimate using integration (rate*time = angle)
// the filter seems to take a while to converge to correct values after fast jitters
// i think i may have forgotten to remove the estimate error value here
angle_estimate[i] += ((dt * gyro_components[i]) - gyro_bias[i]);
}
printf("Angle estimate (INTERNAL): %f %f %f\n", angle_estimate[0], angle_estimate[1], angle_estimate[2]);
printf("Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
raw_gyro_rates[0],
raw_gyro_rates[1],
raw_gyro_rates[2]);
printf("Corrected Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
corrected_rates[0],
corrected_rates[1],
corrected_rates[2]);
printf("PTEMP: %4.8f QTEMP: %4.8f RTEMP: %4.8f\n",
raw_gyro_temperatures[0],
raw_gyro_temperatures[1],
raw_gyro_temperatures[1]);
printf("Accel x: %4.8f (%4.8f volt) y: %4.8f (%4.8f volt) z: %4.8f (%4.8f volt)\n",
raw_accels[0], raw_accels[0]*a2v,
raw_accels[1], raw_accels[1]*a2v,
raw_accels[2], raw_accels[2]*a2v);
printf("2.5v References-Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
raw_gyro_25v[0],
raw_gyro_25v[1],
raw_gyro_25v[2]);
printf("Time ref: %4.8lf Sample freq: %4.8lf\n", dt, 1/dt);
printf("Accelerometer state est: Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
angle_reference_from_accels[0],
angle_reference_from_ac
评论0