mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
modified version of arduimu firmware and driver
This commit is contained in:
Gautier Hattenberger
@@ -2,13 +2,14 @@
|
||||
|
||||
<module name="ins_ArduIMU" dir="ins">
|
||||
<header>
|
||||
<file name="ins_arduimu.h"/>
|
||||
<file name="ins_arduimu_modified.h"/>
|
||||
</header>
|
||||
<init fun="ArduIMU_init()"/>
|
||||
<periodic fun="ArduIMU_periodic()" freq="15" autorun="TRUE"/> <!-- 15 ist soll -->
|
||||
<periodic fun="ArduIMU_periodicGPS()" freq="8" autorun="TRUE"/> <!-- 8 ist soll -->
|
||||
<periodic fun="ArduIMU_periodic()" freq="60"/> <!-- 15 ist soll -->
|
||||
<periodic fun="ArduIMU_periodicGPS()" freq="8"/> <!-- 8 ist soll -->
|
||||
<event fun="ArduIMU_event()"/>
|
||||
<makefile target="ap">
|
||||
<file name="ins_arduimu.c"/>
|
||||
<file name="ins_arduimu_modified.c"/>
|
||||
</makefile>
|
||||
<!--makefile target="sim">
|
||||
<file name="sim_MPPT.c"/>
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
// We are using an oversampling and averaging method to increase the ADC resolution
|
||||
// The theorical ADC resolution is now 11.7 bits. Now we store the ADC readings in float format
|
||||
void Read_adc_raw(void)
|
||||
{
|
||||
int i;
|
||||
uint16_t temp1;
|
||||
uint8_t temp2;
|
||||
|
||||
// ADC readings...
|
||||
for (i=0;i<6;i++)
|
||||
{
|
||||
do{
|
||||
temp1= analog_buffer[sensors[i]]; // sensors[] maps sensors to correct order
|
||||
temp2= analog_count[sensors[i]];
|
||||
} while(temp1 != analog_buffer[sensors[i]]); // Check if there was an ADC interrupt during readings...
|
||||
|
||||
if (temp2>0) AN[i] = float(temp1)/float(temp2); // Check for divide by zero
|
||||
|
||||
}
|
||||
// Initialization for the next readings...
|
||||
for (int i=0;i<8;i++){
|
||||
do{
|
||||
analog_buffer[i]=0;
|
||||
analog_count[i]=0;
|
||||
} while(analog_buffer[i]!=0); // Check if there was an ADC interrupt during initialization...
|
||||
}
|
||||
}
|
||||
|
||||
float read_adc(int select)
|
||||
{
|
||||
float temp;
|
||||
if (SENSOR_SIGN[select]<0){
|
||||
temp = (AN_OFFSET[select]-AN[select]);
|
||||
if (abs(temp)>900) {
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("!!!ADC:1,VAL:");
|
||||
Serial.print (temp);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
adc_constraints++;
|
||||
#endif
|
||||
}
|
||||
return constrain(temp,-900,900); //Throw out nonsensical values
|
||||
} else {
|
||||
temp = (AN[select]-AN_OFFSET[select]);
|
||||
if (abs(temp)>900) {
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("!!!ADC:2,VAL:");
|
||||
Serial.print (temp);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
adc_constraints++;
|
||||
#endif
|
||||
}
|
||||
return constrain(temp,-900,900);
|
||||
}
|
||||
}
|
||||
|
||||
//Activating the ADC interrupts.
|
||||
void Analog_Init(void)
|
||||
{
|
||||
ADCSRA|=(1<<ADIE)|(1<<ADEN);
|
||||
ADCSRA|= (1<<ADSC);
|
||||
}
|
||||
|
||||
//
|
||||
void Analog_Reference(uint8_t mode)
|
||||
{
|
||||
analog_reference = mode;
|
||||
}
|
||||
|
||||
//ADC interrupt vector, this piece of code
|
||||
//is executed everytime a convertion is done.
|
||||
ISR(ADC_vect)
|
||||
{
|
||||
volatile uint8_t low, high;
|
||||
low = ADCL;
|
||||
high = ADCH;
|
||||
|
||||
if(analog_count[MuxSel]<63) {
|
||||
analog_buffer[MuxSel] += (high << 8) | low; // cumulate analog values
|
||||
analog_count[MuxSel]++;
|
||||
}
|
||||
MuxSel++;
|
||||
MuxSel &= 0x07; //if(MuxSel >=8) MuxSel=0;
|
||||
ADMUX = (analog_reference << 6) | MuxSel;
|
||||
// start the conversion
|
||||
ADCSRA|= (1<<ADSC);
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
/* ******************************************************* */
|
||||
/* I2C HMC5843 magnetometer */
|
||||
/* ******************************************************* */
|
||||
|
||||
// Local magnetic declination
|
||||
// I use this web : http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
|
||||
#define MAGNETIC_DECLINATION -6.0 // not used now -> magnetic bearing
|
||||
|
||||
int CompassAddress = 0x1E; //0x3C //0x3D; //(0x42>>1);
|
||||
|
||||
void I2C_Init()
|
||||
{
|
||||
Wire.begin();
|
||||
}
|
||||
|
||||
|
||||
void Compass_Init()
|
||||
{
|
||||
Wire.beginTransmission(CompassAddress);
|
||||
Wire.send(0x02);
|
||||
Wire.send(0x00); // Set continouos mode (default to 10Hz)
|
||||
Wire.endTransmission(); //end transmission
|
||||
}
|
||||
|
||||
void Read_Compass()
|
||||
{
|
||||
int i = 0;
|
||||
byte buff[6];
|
||||
|
||||
Wire.beginTransmission(CompassAddress);
|
||||
Wire.send(0x03); //sends address to read from
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
//Wire.beginTransmission(CompassAddress);
|
||||
Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device
|
||||
while(Wire.available()) // ((Wire.available())&&(i<6))
|
||||
{
|
||||
buff[i] = Wire.receive(); // receive one byte
|
||||
i++;
|
||||
}
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
if (i==6) // All bytes received?
|
||||
{
|
||||
// MSB byte first, then LSB, X,Y,Z
|
||||
magnetom_x = SENSOR_SIGN[6]*((((int)buff[2]) << 8) | buff[3]); // X axis (internal sensor y axis)
|
||||
magnetom_y = SENSOR_SIGN[7]*((((int)buff[0]) << 8) | buff[1]); // Y axis (internal sensor x axis)
|
||||
magnetom_z = SENSOR_SIGN[8]*((((int)buff[4]) << 8) | buff[5]); // Z axis
|
||||
}
|
||||
else
|
||||
Serial.println("!ERR: Mag data");
|
||||
}
|
||||
|
||||
|
||||
void Compass_Heading()
|
||||
{
|
||||
float MAG_X;
|
||||
float MAG_Y;
|
||||
float cos_roll;
|
||||
float sin_roll;
|
||||
float cos_pitch;
|
||||
float sin_pitch;
|
||||
|
||||
cos_roll = cos(roll);
|
||||
sin_roll = sin(roll);
|
||||
cos_pitch = cos(pitch);
|
||||
sin_pitch = sin(pitch);
|
||||
// Tilt compensated Magnetic filed X:
|
||||
MAG_X = magnetom_x*cos_pitch+magnetom_y*sin_roll*sin_pitch+magnetom_z*cos_roll*sin_pitch;
|
||||
// Tilt compensated Magnetic filed Y:
|
||||
MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
|
||||
// Magnetic Heading
|
||||
MAG_Heading = atan2(-MAG_Y,MAG_X);
|
||||
}
|
||||
@@ -0,0 +1,280 @@
|
||||
/**************************************************/
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
float temporary[3][3];
|
||||
float renorm=0;
|
||||
boolean problem=FALSE;
|
||||
|
||||
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
|
||||
|
||||
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
|
||||
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
|
||||
|
||||
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
|
||||
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
|
||||
|
||||
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???SQT:1,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:1,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???SQT:2,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:2,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???SQT:3,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:3,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
|
||||
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
|
||||
DCM_Matrix[0][0]= 1.0f;
|
||||
DCM_Matrix[0][1]= 0.0f;
|
||||
DCM_Matrix[0][2]= 0.0f;
|
||||
DCM_Matrix[1][0]= 0.0f;
|
||||
DCM_Matrix[1][1]= 1.0f;
|
||||
DCM_Matrix[1][2]= 0.0f;
|
||||
DCM_Matrix[2][0]= 0.0f;
|
||||
DCM_Matrix[2][1]= 0.0f;
|
||||
DCM_Matrix[2][2]= 1.0f;
|
||||
problem = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
void Drift_correction(void)
|
||||
{
|
||||
//Compensation the Roll, Pitch and Yaw drift.
|
||||
float mag_heading_x;
|
||||
float mag_heading_y;
|
||||
static float Scaled_Omega_P[3];
|
||||
static float Scaled_Omega_I[3];
|
||||
float Accel_magnitude;
|
||||
float Accel_weight;
|
||||
float Integrator_magnitude;
|
||||
float tempfloat;
|
||||
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
// Calculate the magnitude of the accelerometer vector
|
||||
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
|
||||
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
// Dynamic weighting of accelerometer info (reliability filter)
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
|
||||
imu_health += tempfloat;
|
||||
imu_health = constrain(imu_health,129,65405);
|
||||
#endif
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
|
||||
|
||||
//*****YAW***************
|
||||
|
||||
#if USE_MAGNETOMETER==1
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
mag_heading_x = cos(MAG_Heading);
|
||||
mag_heading_y = sin(MAG_Heading);
|
||||
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
#else // Use GPS Ground course to correct yaw gyro drift
|
||||
if(ground_speed>=SPEEDFILT)
|
||||
{
|
||||
COGX = cos(ToRad(ground_course));
|
||||
COGY = sin(ToRad(ground_course));
|
||||
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
}
|
||||
#endif
|
||||
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
|
||||
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
|
||||
if (Integrator_magnitude > ToRad(300)) {
|
||||
Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("!!!INT:1,MAG:");
|
||||
Serial.print (ToDeg(Integrator_magnitude));
|
||||
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/**************************************************/
|
||||
void Accel_adjust(void)
|
||||
{
|
||||
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
|
||||
Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
|
||||
Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
|
||||
|
||||
Accel_Vector[0]=read_adc(3); // acc x
|
||||
Accel_Vector[1]=read_adc(4); // acc y
|
||||
Accel_Vector[2]=read_adc(5); // acc z
|
||||
|
||||
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
|
||||
|
||||
Accel_adjust(); //Remove centrifugal acceleration.
|
||||
|
||||
#if OUTPUTMODE==1
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
|
||||
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
|
||||
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
|
||||
Update_Matrix[2][2]=0;
|
||||
#else // Uncorrected data (no drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
|
||||
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
for(int x=0; x<3; x++) //Matrix Addition (update)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Euler_angles(void)
|
||||
{
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
|
||||
yaw = 0;
|
||||
#else
|
||||
pitch = -asin(DCM_Matrix[2][0]);
|
||||
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -0,0 +1,204 @@
|
||||
#if GPS_PROTOCOL == 2
|
||||
|
||||
#define BUF_LEN 100
|
||||
|
||||
// The input buffer
|
||||
char gps_buffer[BUF_LEN]={
|
||||
|
||||
// Setup for SIRF binary at 38400 - $PSRF100,0,38400,8,1,0*3C<cr><lf>
|
||||
0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x33,0x38,0x34,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x43,0x0D,0x0A};
|
||||
|
||||
// Used to configure Sirf GPS
|
||||
const byte gps_ender[]={0xB0,0xB3};
|
||||
|
||||
/****************************************************************
|
||||
Parsing stuff for SIRF binary protocol.
|
||||
****************************************************************/
|
||||
void init_gps(void)
|
||||
{
|
||||
pinMode(2,OUTPUT); //Serial Mux
|
||||
digitalWrite(2,HIGH); //Serial Mux
|
||||
change_to_sirf_protocol();
|
||||
delay(100);//Waits fot the GPS to start_UP
|
||||
configure_gps();//Function to configure GPS, to output only the desired msg's
|
||||
}
|
||||
|
||||
void decode_gps(void)
|
||||
{
|
||||
static unsigned long GPS_timer = 0;
|
||||
static byte gps_counter = 0; //Another gps counter for the buffer
|
||||
static byte GPS_step = 0;
|
||||
boolean gps_failure = false;
|
||||
static byte gps_ok = 0;//Counter to verify the reciving info
|
||||
const byte read_gps_header[]={0xA0,0xA2,0x00,0x5B,0x29};//Used to verify the payload msg header
|
||||
|
||||
if(Serial.available() > 0)//Ok, let me see, the buffer is empty?
|
||||
{
|
||||
while(Serial.available() < 5){ }
|
||||
switch(GPS_step) {
|
||||
case 0: //This case will verify the header, to know when the payload will begin
|
||||
while(Serial.available() > 0) //Loop if data available
|
||||
{
|
||||
if(Serial.read() == read_gps_header[gps_ok]){ //Checking if the head bytes are equal..
|
||||
//if yes increment 1
|
||||
gps_ok++;
|
||||
}else{
|
||||
//Otherwise restart.
|
||||
gps_ok = 0;
|
||||
}
|
||||
if(gps_ok >= 5) {
|
||||
//Ohh 5 bytes are correct, that means jump to the next step, and break the loop
|
||||
gps_ok = 0;
|
||||
GPS_step++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1: //Will receive all the payload and join the received bytes...
|
||||
while(Serial.available() < 92){
|
||||
}
|
||||
gps_counter = 0;
|
||||
memset(gps_buffer,0,sizeof(gps_buffer));
|
||||
|
||||
while(Serial.available() > 0){
|
||||
//Read data and store it in the temp buffer
|
||||
byte b1 = Serial.read();
|
||||
byte b2 = gps_buffer[gps_counter-1];
|
||||
// gps_ender[]={0xB0,0xB3};
|
||||
|
||||
if((b1 == gps_ender[1]) && (b2 == gps_ender[0])){
|
||||
GPS_step = 0;
|
||||
gps_counter = 0;
|
||||
gpsFix = gps_buffer[1];
|
||||
|
||||
if(gpsFix == 0x00){
|
||||
// GPS signal is error free
|
||||
// ------------------------
|
||||
digitalWrite(6,HIGH);
|
||||
GPS_timer = millis();
|
||||
gpsFixnew=1; //new information available flag for binary message
|
||||
|
||||
//Parse the data
|
||||
GPS_join_data();
|
||||
|
||||
} else {
|
||||
// GPS has returned an error code
|
||||
// ------------------------------
|
||||
gpsFix = 0x01; // In GPS language a good fix = 0, bad = 1
|
||||
digitalWrite(6,LOW);
|
||||
}
|
||||
|
||||
break;
|
||||
}else{
|
||||
gps_buffer[gps_counter] = b1;
|
||||
gps_counter++;
|
||||
|
||||
if (gps_counter >= BUF_LEN){
|
||||
Serial.flush();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if(millis() - GPS_timer > 2000){
|
||||
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
|
||||
gpsFix = 0x01;
|
||||
}
|
||||
}
|
||||
|
||||
void GPS_join_data(void)
|
||||
{
|
||||
// Read bytes and combine them with Unions
|
||||
// ---------------------------------------
|
||||
byte j = 22;
|
||||
longUnion.byte[3] = gps_buffer[j++];
|
||||
longUnion.byte[2] = gps_buffer[j++];
|
||||
longUnion.byte[1] = gps_buffer[j++];
|
||||
longUnion.byte[0] = gps_buffer[j++];
|
||||
lat = longUnion.dword; // latitude * 10,000,000
|
||||
|
||||
|
||||
longUnion.byte[3] = gps_buffer[j++];
|
||||
longUnion.byte[2] = gps_buffer[j++];
|
||||
longUnion.byte[1] = gps_buffer[j++];
|
||||
longUnion.byte[0] = gps_buffer[j++];
|
||||
lon = longUnion.dword; // longitude * 10,000,000
|
||||
|
||||
j = 34;
|
||||
longUnion.byte[3] = gps_buffer[j++];
|
||||
longUnion.byte[2] = gps_buffer[j++];
|
||||
longUnion.byte[1] = gps_buffer[j++];
|
||||
longUnion.byte[0] = gps_buffer[j++];
|
||||
alt_MSL = longUnion.dword * 10; // alt in meters * 1000
|
||||
|
||||
j = 39;
|
||||
intUnion.byte[1] = gps_buffer[j++];
|
||||
intUnion.byte[0] = gps_buffer[j++];
|
||||
speed_3d = (float)intUnion.word / 100.0; // meters/second We only get ground speed but store it as speed_3d for use in DCM
|
||||
|
||||
//iTOW
|
||||
|
||||
intUnion.byte[1] = gps_buffer[j++];
|
||||
intUnion.byte[0] = gps_buffer[j++];
|
||||
ground_course = (float)intUnion.word / 100.0; // degrees
|
||||
ground_course = abs(ground_course); //The GPS has a BUG sometimes give you the correct value but negative, weird!!
|
||||
|
||||
// clear buffer
|
||||
// -------------
|
||||
memset(gps_buffer,0,sizeof(gps_buffer));
|
||||
}
|
||||
|
||||
|
||||
void configure_gps(void)
|
||||
{
|
||||
const byte gps_header[]={
|
||||
0xA0,0xA2,0x00,0x08,0xA6,0x00 };//Used to configure Sirf GPS
|
||||
const byte gps_payload[]={
|
||||
0x02,0x04,0x07,0x09,0x1B };//Used to configure Sirf GPS
|
||||
const byte gps_checksum[]={
|
||||
0xA8,0xAA,0xAD,0xAF,0xC1 };//Used to configure Sirf GPS
|
||||
const byte cero = 0x00;//Used to configure Sirf GPS
|
||||
|
||||
for(int z=0; z<2; z++)
|
||||
{
|
||||
for(int x=0; x<5; x++)//Print all messages to setup GPS
|
||||
{
|
||||
for(int y=0; y<6; y++)
|
||||
{
|
||||
Serial.print(byte(gps_header[y]));//Prints the msg header, is the same header for all msg..
|
||||
}
|
||||
Serial.print(byte(gps_payload[x]));//Prints the payload, is not the same for every msg
|
||||
for(int y=0; y<6; y++)
|
||||
{
|
||||
Serial.print(byte(cero)); //Prints 6 zeros
|
||||
}
|
||||
Serial.print(byte(gps_checksum[x])); //Print the Checksum
|
||||
Serial.print(byte(gps_ender[0])); //Print the Ender of the string, is same on all msg's.
|
||||
Serial.print(byte(gps_ender[1])); //ender
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void change_to_sirf_protocol(void)
|
||||
{
|
||||
Serial.begin(4800); //First try in 4800
|
||||
delay(300);
|
||||
for (byte x=0; x<=28; x++)
|
||||
{
|
||||
Serial.print(byte(gps_buffer[x]));//Sending special bytes declared at the beginning
|
||||
}
|
||||
delay(300);
|
||||
Serial.begin(9600); //Then try in 9600
|
||||
delay(300);
|
||||
for (byte x=0; x<=28; x++)
|
||||
{
|
||||
Serial.print(byte(gps_buffer[x]));
|
||||
}
|
||||
Serial.begin(38400); //Universal Synchronous Asynchronous Recieving Transmiting
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,292 @@
|
||||
#if GPS_PROTOCOL == 1
|
||||
|
||||
//*********************************************************************************************
|
||||
// You may need to insert parameters appropriate for your gps from this list into init_gps()
|
||||
//GPS SIRF configuration strings...
|
||||
#define SIRF_BAUD_RATE_4800 "$PSRF100,1,4800,8,1,0*0E\r\n"
|
||||
#define SIRF_BAUD_RATE_9600 "$PSRF100,1,9600,8,1,0*0D\r\n"
|
||||
#define SIRF_BAUD_RATE_19200 "$PSRF100,1,19200,8,1,0*38\r\n"
|
||||
#define SIRF_BAUD_RATE_38400 "$PSRF100,1,38400,8,1,0*3D\r\n"
|
||||
#define SIRF_BAUD_RATE_57600 "$PSRF100,1,57600,8,1,0*36\r\n"
|
||||
#define GSA_ON "$PSRF103,2,0,1,1*27\r\n" // enable GSA
|
||||
#define GSA_OFF "$PSRF103,2,0,0,1*26\r\n" // disable GSA
|
||||
#define GSV_ON "$PSRF103,3,0,1,1*26\r\n" // enable GSV
|
||||
#define GSV_OFF "$PSRF103,3,0,0,1*27\r\n" // disable GSV
|
||||
#define USE_WAAS 1 //1 = Enable, 0 = Disable, good in USA, slower FIX...
|
||||
#define WAAS_ON "$PSRF151,1*3F\r\n" // enable WAAS
|
||||
#define WAAS_OFF "$PSRF151,0*3E\r\n" // disable WAAS
|
||||
|
||||
//GPS Locosys configuration strings...
|
||||
#define USE_SBAS 0
|
||||
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
||||
#define SBAS_OFF "$PMTK313,0*2F\r\n"
|
||||
|
||||
#define NMEA_OUTPUT_5HZ "$PMTK314,0,5,0,5,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 5HZ
|
||||
#define NMEA_OUTPUT_4HZ "$PMTK314,0,4,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 4HZ
|
||||
#define NMEA_OUTPUT_3HZ "$PMTK314,0,3,0,3,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 3HZ
|
||||
#define NMEA_OUTPUT_2HZ "$PMTK314,0,2,0,2,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 2HZ
|
||||
#define NMEA_OUTPUT_1HZ "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 1HZ
|
||||
|
||||
#define LOCOSYS_REFRESH_RATE_200 "$PMTK220,200*2C" //200 milliseconds
|
||||
#define LOCOSYS_REFRESH_RATE_250 "$PMTK220,250*29\r\n" //250 milliseconds
|
||||
|
||||
#define LOCOSYS_BAUD_RATE_4800 "$PMTK251,4800*14\r\n"
|
||||
#define LOCOSYS_BAUD_RATE_9600 "$PMTK251,9600*17\r\n"
|
||||
#define LOCOSYS_BAUD_RATE_19200 "$PMTK251,19200*22\r\n"
|
||||
#define LOCOSYS_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
|
||||
//**************************************************************************************************************
|
||||
|
||||
|
||||
/****************************************************************
|
||||
Parsing stuff for NMEA
|
||||
****************************************************************/
|
||||
#define BUF_LEN 200
|
||||
|
||||
// GPS Pointers
|
||||
char *token;
|
||||
char *search = ",";
|
||||
char *brkb, *pEnd;
|
||||
char gps_buffer[BUF_LEN]; //The traditional buffer.
|
||||
|
||||
void init_gps(void)
|
||||
{
|
||||
pinMode(2,OUTPUT); //Serial Mux
|
||||
digitalWrite(2,HIGH); //Serial Mux
|
||||
Serial.begin(9600);
|
||||
delay(1000);
|
||||
Serial.print(LOCOSYS_BAUD_RATE_38400);
|
||||
Serial.begin(38400);
|
||||
delay(500);
|
||||
Serial.print(LOCOSYS_REFRESH_RATE_250);
|
||||
delay(500);
|
||||
Serial.print(NMEA_OUTPUT_4HZ);
|
||||
delay(500);
|
||||
Serial.print(SBAS_OFF);
|
||||
|
||||
|
||||
/* EM406 example init
|
||||
Serial.begin(4800); //Universal Sincronus Asyncronus Receiveing Transmiting
|
||||
delay(1000);
|
||||
Serial.print(SIRF_BAUD_RATE_9600);
|
||||
|
||||
Serial.begin(9600);
|
||||
delay(1000);
|
||||
|
||||
Serial.print(GSV_OFF);
|
||||
Serial.print(GSA_OFF);
|
||||
|
||||
#if USE_WAAS == 1
|
||||
Serial.print(WAAS_ON);
|
||||
#else
|
||||
Serial.print(WAAS_OFF);
|
||||
#endif
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
void decode_gps(void)
|
||||
{
|
||||
const char head_rmc[]="GPRMC"; //GPS NMEA header to look for
|
||||
const char head_gga[]="GPGGA"; //GPS NMEA header to look for
|
||||
|
||||
static unsigned long GPS_timer = 0; //used to turn off the LED if no data is received.
|
||||
|
||||
static byte unlock = 1; //some kind of event flag
|
||||
static byte checksum = 0; //the checksum generated
|
||||
static byte checksum_received = 0; //Checksum received
|
||||
static byte counter = 0; //general counter
|
||||
|
||||
//Temporary variables for some tasks, specially used in the GPS parsing part (Look at the NMEA_Parser tab)
|
||||
unsigned long temp = 0;
|
||||
unsigned long temp2 = 0;
|
||||
unsigned long temp3 = 0;
|
||||
|
||||
|
||||
while(Serial.available() > 0)
|
||||
{
|
||||
if(unlock == 0)
|
||||
{
|
||||
gps_buffer[0] = Serial.read();//puts a byte in the buffer
|
||||
|
||||
if(gps_buffer[0]=='$')//Verify if is the preamble $
|
||||
{
|
||||
counter = 0;
|
||||
checksum = 0;
|
||||
unlock = 1;
|
||||
}
|
||||
} else {
|
||||
gps_buffer[counter] = Serial.read();
|
||||
|
||||
if(gps_buffer[counter] == 0x0A)//Looks for \F
|
||||
{
|
||||
unlock = 0;
|
||||
|
||||
if (strncmp (gps_buffer, head_rmc, 5) == 0)//looking for rmc head....
|
||||
{
|
||||
|
||||
/*Generating and parsing received checksum, */
|
||||
for(int x=0; x<100; x++)
|
||||
{
|
||||
if(gps_buffer[x]=='*')
|
||||
{
|
||||
checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);//Parsing received checksum...
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
checksum ^= gps_buffer[x]; //XOR the received data...
|
||||
}
|
||||
}
|
||||
|
||||
if(checksum_received == checksum)//Checking checksum
|
||||
{
|
||||
/* Token will point to the data between comma "'", returns the data in the order received */
|
||||
/*THE GPRMC order is: UTC, UTC status , Lat, N/S indicator, Lon, E/W indicator, speed, course, date, mode, checksum*/
|
||||
token = strtok_r(gps_buffer, search, &brkb); //Contains the header GPRMC, not used
|
||||
|
||||
token = strtok_r(NULL, search, &brkb); //UTC Time, not used
|
||||
//time= atol (token);
|
||||
token = strtok_r(NULL, search, &brkb); //Valid UTC data? maybe not used...
|
||||
|
||||
|
||||
//Longitude in degrees, decimal minutes. (ej. 4750.1234 degrees decimal minutes = 47.835390 decimal degrees)
|
||||
//Where 47 are degrees and 50 the minutes and .1234 the decimals of the minutes.
|
||||
//To convert to decimal degrees, devide the minutes by 60 (including decimals),
|
||||
//Example: "50.1234/60=.835390", then add the degrees, ex: "47+.835390 = 47.835390" decimal degrees
|
||||
token = strtok_r(NULL, search, &brkb); //Contains Latitude in degrees decimal minutes...
|
||||
|
||||
//taking only degrees, and minutes without decimals,
|
||||
//strtol stop parsing till reach the decimal point "." result example 4750, eliminates .1234
|
||||
temp = strtol (token, &pEnd, 10);
|
||||
|
||||
//takes only the decimals of the minutes
|
||||
//result example 1234.
|
||||
temp2 = strtol (pEnd + 1, NULL, 10);
|
||||
|
||||
//joining degrees, minutes, and the decimals of minute, now without the point...
|
||||
//Before was 4750.1234, now the result example is 47501234...
|
||||
temp3 = (temp * 10000) + (temp2);
|
||||
|
||||
|
||||
//modulo to leave only the decimal minutes, eliminating only the degrees..
|
||||
//Before was 47501234, the result example is 501234.
|
||||
temp3 = temp3 % 1000000;
|
||||
|
||||
|
||||
//Dividing to obtain only the de degrees, before was 4750
|
||||
//The result example is 47 (4750/100 = 47)
|
||||
temp /= 100;
|
||||
|
||||
//Joining everything and converting to * 10,000,000 ...
|
||||
//First i convert the decimal minutes to degrees decimals stored in "temp3", example: 501234/600000 =.835390
|
||||
//Then i add the degrees stored in "temp" and add the result from the first step, example 47+.835390 = 47.835390
|
||||
//The result is stored in "lat" variable...
|
||||
//**This is all changed in this case to be a long integer which is decimal degrees * 10**7
|
||||
|
||||
lat = (temp * 10000000) + ((temp3 *100) / 6);
|
||||
|
||||
token = strtok_r(NULL, search, &brkb); //lat, north or south?
|
||||
//If the char is equal to S (south), multiply the result by -1..
|
||||
if(*token == 'S'){
|
||||
lat *= -1;
|
||||
}
|
||||
|
||||
//This the same procedure use in lat, but now for Lon....
|
||||
token = strtok_r(NULL, search, &brkb);
|
||||
temp = strtol (token,&pEnd, 10);
|
||||
temp2 = strtol (pEnd + 1, NULL, 10);
|
||||
temp3 = (temp * 10000) + (temp2);
|
||||
temp3 = temp3%1000000;
|
||||
temp/= 100;
|
||||
lon = (temp * 10000000) + ((temp3 * 100) / 6);
|
||||
|
||||
token = strtok_r(NULL, search, &brkb); //lon, east or west?
|
||||
if(*token == 'W'){
|
||||
lon *= -1;
|
||||
}
|
||||
|
||||
token = strtok_r(NULL, search, &brkb); //Speed over ground
|
||||
speed_3d = (float)atoi(token); // We only get ground speed but store it as speed_3d for use in DCM
|
||||
|
||||
token = strtok_r(NULL, search, &brkb); //Course
|
||||
ground_course = (float)atoi(token);
|
||||
|
||||
gpsFixnew=1; //new information available flag for binary message
|
||||
|
||||
}
|
||||
checksum = 0;
|
||||
}//End of the GPRMC parsing
|
||||
|
||||
if (strncmp (gps_buffer, head_gga, 5) == 0)//now looking for GPGGA head....
|
||||
{
|
||||
/*Generating and parsing received checksum, */
|
||||
for(int x = 0; x<100; x++)
|
||||
{
|
||||
if(gps_buffer[x]=='*')
|
||||
{
|
||||
checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);//Parsing received checksum...
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
checksum^= gps_buffer[x]; //XOR the received data...
|
||||
}
|
||||
}
|
||||
|
||||
if(checksum_received== checksum)//Checking checksum
|
||||
{
|
||||
//strcpy(gps_GGA,gps_buffer);
|
||||
|
||||
token = strtok_r(gps_buffer, search, &brkb);//GPGGA header, not used anymore
|
||||
token = strtok_r(NULL, search, &brkb);//UTC, not used!!
|
||||
token = strtok_r(NULL, search, &brkb);//lat, not used!!
|
||||
token = strtok_r(NULL, search, &brkb);//north/south, nope...
|
||||
token = strtok_r(NULL, search, &brkb);//lon, not used!!
|
||||
token = strtok_r(NULL, search, &brkb);//wets/east, nope
|
||||
token = strtok_r(NULL, search, &brkb);//Position fix, used!!
|
||||
|
||||
if(atoi(token) >= 1){
|
||||
gpsFix = 0x00; // gpsFix = 0 means valid fix
|
||||
}else{
|
||||
gpsFix = 0x01;
|
||||
}
|
||||
token = strtok_r(NULL, search, &brkb); //sats in use!! Nein...
|
||||
token = strtok_r(NULL, search, &brkb);//HDOP, not needed
|
||||
token = strtok_r(NULL, search, &brkb);//ALTITUDE, is the only meaning of this string.. in meters of course.
|
||||
alt_MSL = abs(atoi(token)) * 1000;
|
||||
|
||||
if(gpsFix == 0x00) digitalWrite(6, HIGH); //Status LED...
|
||||
else digitalWrite(6, LOW);
|
||||
}
|
||||
checksum = 0; //Restarting the checksum
|
||||
}
|
||||
|
||||
for(int a = 0; a<= counter; a++)//restarting the buffer
|
||||
{
|
||||
gps_buffer[a]= 0;
|
||||
}
|
||||
counter = 0; //Restarting the counter
|
||||
GPS_timer = millis(); //Restarting timer...
|
||||
}
|
||||
else
|
||||
{
|
||||
counter++; //Incrementing counter
|
||||
if (counter >= 200)
|
||||
{
|
||||
//Serial.flush();
|
||||
counter = 0;
|
||||
checksum = 0;
|
||||
unlock = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(millis() - GPS_timer > 2000){
|
||||
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
|
||||
gpsFix = 0x01;
|
||||
gpsFixnew = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,177 @@
|
||||
#if GPS_PROTOCOL == 3
|
||||
|
||||
/****************************************************************
|
||||
* Here you have all the parsing stuff for uBlox
|
||||
****************************************************************/
|
||||
// Code from Jordi, modified by Jose
|
||||
|
||||
//You have to disable all the other string, only leave this ones:
|
||||
|
||||
//NAV-POSLLH Geodetic Position Solution, PAGE 66 of datasheet
|
||||
//NAV-VELNED Velocity Solution in NED, PAGE 71 of datasheet
|
||||
//NAV-STATUS Receiver Navigation Status, PAGE 67 of datasheet
|
||||
//NAV-SOL Navigation Solution Information, PAGE 72 of datasheet
|
||||
|
||||
|
||||
// Baud Rate:38400
|
||||
|
||||
/*
|
||||
GPSfix Type
|
||||
- 0x00 = no fix
|
||||
- 0x01 = dead reckonin
|
||||
- 0x02 = 2D-fix
|
||||
- 0x03 = 3D-fix
|
||||
- 0x04 = GPS + dead re
|
||||
- 0x05 = Time only fix
|
||||
- 0x06..0xff = reserved*/
|
||||
|
||||
//Luckly uBlox has internal EEPROM so all the settings you change will remain forever. Not like the SIRF modules!
|
||||
|
||||
void init_gps(void)
|
||||
{
|
||||
//Serial.begin(38400);
|
||||
pinMode(2,OUTPUT); //Serial Mux
|
||||
digitalWrite(2,HIGH); //Serial Mux
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
|
||||
void decode_gps(void){
|
||||
|
||||
|
||||
|
||||
if(DIYmillis() - GPS_timer > 2000){
|
||||
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
|
||||
debug_print("Yeah, your GPS is disconnected");
|
||||
gpsFix=1;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
void parse_ubx_gps(){
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
gps_pos_fix_count++;
|
||||
#endif
|
||||
|
||||
messageNr = Paparazzi_GPS_buffer[0];
|
||||
|
||||
if(messageNr == 0x00){
|
||||
//Nachricht 0 Bytes:
|
||||
iTOW2 = join_4_bytes(&Paparazzi_GPS_buffer[1]); //1,2,3,4
|
||||
lon2 = join_4_bytes(&Paparazzi_GPS_buffer[5]); //5,6,7,8
|
||||
lat2 = join_4_bytes(&Paparazzi_GPS_buffer[9]); //9,10,11,12
|
||||
alt2 = join_4_bytes(&Paparazzi_GPS_buffer[13]); //13,14,15,16
|
||||
alt_MSL2 = join_4_bytes(&Paparazzi_GPS_buffer[17]); // 17,18,19,20
|
||||
speed_3d2 = (float)join_4_bytes(&Paparazzi_GPS_buffer[21])/100.0; // m/s 21,22,23,24
|
||||
ground_speed2 = (float)join_4_bytes(&Paparazzi_GPS_buffer[25])/100.0; // Ground speed 2D 25,26,27,28 29,30 31
|
||||
recPakOne=0x01;
|
||||
}
|
||||
|
||||
|
||||
if(messageNr == 0x01 && recPakOne==0x01){
|
||||
// Nachricht 1
|
||||
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[1])/100000.0; // Heading 2D 1,2,3,4
|
||||
ecefVZ=(float)join_4_bytes(&Paparazzi_GPS_buffer[5])/100; //Vertical Speed 5,6,7,8
|
||||
numSV=Paparazzi_GPS_buffer[9]; //Number of sats... 9
|
||||
stGpsFix=Paparazzi_GPS_buffer[10];
|
||||
stFlags=Paparazzi_GPS_buffer[11];
|
||||
solGpsFix=Paparazzi_GPS_buffer[12];
|
||||
solFlags=Paparazzi_GPS_buffer[13];
|
||||
|
||||
iTOW = iTOW2;
|
||||
lon = lon2;
|
||||
lat = lat2;
|
||||
alt = alt2;
|
||||
alt_MSL = alt_MSL2;
|
||||
speed_3d = speed_3d2;
|
||||
ground_speed = ground_speed2;
|
||||
|
||||
messageNr=messageNr+1; //2
|
||||
}
|
||||
|
||||
|
||||
if(messageNr == 0x02){
|
||||
if((stGpsFix >= 0x03)&&(stFlags&0x01)){
|
||||
gpsFix=0; //valid position
|
||||
digitalWrite(6,HIGH); //Turn LED when gps is fixed.
|
||||
GPS_timer=DIYmillis(); //Restarting timer...
|
||||
}
|
||||
else{
|
||||
gpsFix=1; //invalid position
|
||||
digitalWrite(6,LOW);
|
||||
}
|
||||
|
||||
if((solGpsFix >= 0x03)&&(solFlags&0x01)){
|
||||
gpsFix=0; //valid position
|
||||
digitalWrite(6,HIGH); //Turn LED when gps is fixed.
|
||||
GPS_timer=DIYmillis(); //Restarting timer...
|
||||
}
|
||||
else{
|
||||
gpsFix=1; //invalid position
|
||||
digitalWrite(6,LOW);
|
||||
}
|
||||
|
||||
if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
|
||||
recPakOne=0x00;
|
||||
//messageNr= 0x05; // kommt so nicht mehr in die Abfage !!!
|
||||
#if 0
|
||||
// Serial.print("Time von Arduino ;");
|
||||
// Serial.print(millis());
|
||||
Serial.print("MesageNr: ");
|
||||
Serial.print((int)(messageNr));
|
||||
Serial.print("; itow ;");
|
||||
Serial.print(iTOW);
|
||||
Serial.print("; lon ;");
|
||||
Serial.print(lon);
|
||||
Serial.print("; lat ;");
|
||||
Serial.print(lat);
|
||||
Serial.print("; alt ;");
|
||||
Serial.print(alt);
|
||||
Serial.print("; alt_MSL: ;");
|
||||
Serial.print(alt_MSL);
|
||||
Serial.print("; speed_3d ;");
|
||||
Serial.print(speed_3d);
|
||||
Serial.print("; ground_speed ;");
|
||||
Serial.print(ground_speed);
|
||||
Serial.print("; ground_course ;");
|
||||
Serial.print(ground_course);
|
||||
Serial.print("; ecefVZ ;");
|
||||
Serial.print(ecefVZ);
|
||||
Serial.print("; numSV ;");
|
||||
Serial.println((int)(numSV));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
// Join 4 bytes into a long
|
||||
int32_t join_4_bytes(byte Buffer[])
|
||||
{
|
||||
longUnion.byte[0] = *Buffer;
|
||||
longUnion.byte[1] = *(Buffer+1);
|
||||
longUnion.byte[2] = *(Buffer+2);
|
||||
longUnion.byte[3] = *(Buffer+3);
|
||||
return(longUnion.dword);
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
void checksum(byte ubx_data)
|
||||
{
|
||||
ck_a+=ubx_data;
|
||||
ck_b+=ck_a;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,429 @@
|
||||
|
||||
|
||||
//*****I2C Output************************************************************
|
||||
// PRINT_I2C_Data
|
||||
// #if PRINT_BINARY != 1 //Print either Ascii or binary messages
|
||||
|
||||
void requestEvent(){
|
||||
// Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
|
||||
// Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
|
||||
// Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
|
||||
I2C_Message_ar[0] = int(roll*(1<<12));
|
||||
I2C_Message_ar[1] = int(pitch*(1<<12));
|
||||
I2C_Message_ar[2] = int(yaw*(1<<12));
|
||||
I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
|
||||
I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
|
||||
I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
|
||||
I2C_Message_ar[6] = int(Accel_Vector[0]*(1<<10));
|
||||
I2C_Message_ar[7] = int(Accel_Vector[1]*(1<<10));
|
||||
I2C_Message_ar[8] = int(Accel_Vector[2]*(1<<10));
|
||||
|
||||
byte* pointer;
|
||||
pointer = (byte*) &I2C_Message_ar;
|
||||
Wire.send(pointer, 12);
|
||||
|
||||
/* Uncomment for debug on serial link */
|
||||
/*
|
||||
//Serial.println();
|
||||
Serial.print("Time ;");
|
||||
Serial.print(millis());
|
||||
Serial.print("; Roll ;");
|
||||
Serial.print(I2C_Message_ar[0]);
|
||||
Serial.print("; Pitch ;");
|
||||
Serial.print(I2C_Message_ar[1]);
|
||||
Serial.print("; YaW ;");
|
||||
Serial.print(I2C_Message_ar[2]);
|
||||
Serial.print("; GyroX ;");
|
||||
Serial.print(I2C_Message_ar[3]);
|
||||
Serial.print("; GyroY: ;");
|
||||
Serial.print(I2C_Message_ar[4]);
|
||||
Serial.print("; GyroZ ;");
|
||||
Serial.print(I2C_Message_ar[5]);
|
||||
Serial.print("; ACCX ;");
|
||||
Serial.print(I2C_Message_ar[6]);
|
||||
Serial.print("; ACCY: ;");
|
||||
Serial.print(I2C_Message_ar[7]);
|
||||
Serial.print("; ACCZ ;");
|
||||
Serial.println(I2C_Message_ar[8]);
|
||||
*/
|
||||
}
|
||||
|
||||
//********GPS Data from PAPArazzi UBLOX**********************************************************************
|
||||
|
||||
void receiveEvent(int howMany){
|
||||
Serial.print(" How Many Bytes GPS : ");
|
||||
Serial.println(howMany);
|
||||
|
||||
for(int i=0; i < howMany; i++){
|
||||
Paparazzi_GPS_buffer[i]=Wire.receive();
|
||||
}
|
||||
|
||||
parse_ubx_gps(); // Parse new GPS packet...
|
||||
GPS_timer=DIYmillis(); //Restarting timer...
|
||||
|
||||
gpsDataReady=1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
//*************************************************************************************************************
|
||||
|
||||
|
||||
void printdata(void){
|
||||
|
||||
|
||||
#if PRINT_I2C_Data == 1
|
||||
/*
|
||||
I2C_Message_ar[0] = int(roll*(1<<12));
|
||||
I2C_Message_ar[1] = int(pitch*(1<<12));
|
||||
I2C_Message_ar[2] = int(yaw*(1<<12));
|
||||
I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
|
||||
I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
|
||||
I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
|
||||
I2C_Message_ar[6] = int(Accel_Vector[0]*(1<<10));
|
||||
I2C_Message_ar[7] = int(Accel_Vector[1]*(1<<10));
|
||||
I2C_Message_ar[8] = int(Accel_Vector[2]*(1<<10));
|
||||
|
||||
Serial.println();
|
||||
Serial.print("Time ;");
|
||||
Serial.print(millis());
|
||||
Serial.print("; Roll ;");
|
||||
Serial.print(I2C_Message_ar[0]);
|
||||
Serial.print("; Pitch ;");
|
||||
Serial.print(I2C_Message_ar[1]);
|
||||
Serial.print("; YaW ;");
|
||||
Serial.print(I2C_Message_ar[2]);
|
||||
Serial.print("; GyroX ;");
|
||||
Serial.print(I2C_Message_ar[3]);
|
||||
Serial.print("; GyroY: ;");
|
||||
Serial.print(I2C_Message_ar[4]);
|
||||
Serial.print("; GyroZ ;");
|
||||
Serial.print(I2C_Message_ar[5]);
|
||||
Serial.print("; ACCX ;");
|
||||
Serial.print(I2C_Message_ar[6]);
|
||||
Serial.print("; ACCY: ;");
|
||||
Serial.print(I2C_Message_ar[7]);
|
||||
Serial.print("; ACCZ ;");
|
||||
Serial.println(I2C_Message_ar[8]);
|
||||
*/
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if PRINT_BINARY != 1 //Print either Ascii or binary messages
|
||||
|
||||
//Serial.print("!!!VER:");
|
||||
//Serial.print(SOFTWARE_VER); //output the software version
|
||||
//Serial.print(",");
|
||||
|
||||
#if PRINT_ANALOGS==1
|
||||
Serial.print("AN0:");
|
||||
Serial.print(read_adc(0)); //Reversing the sign.
|
||||
Serial.print(",AN1:");
|
||||
Serial.print(read_adc(1));
|
||||
Serial.print(",AN2:");
|
||||
Serial.print(read_adc(2));
|
||||
Serial.print(",AN3:");
|
||||
Serial.print(read_adc(3));
|
||||
Serial.print (",AN4:");
|
||||
Serial.print(read_adc(4));
|
||||
Serial.print (",AN5:");
|
||||
Serial.print(read_adc(5));
|
||||
Serial.print (",");
|
||||
#endif
|
||||
|
||||
#if PRINT_DCM == 1
|
||||
Serial.print ("EX0:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[0][0]));
|
||||
Serial.print (",EX1:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[0][1]));
|
||||
Serial.print (",EX2:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[0][2]));
|
||||
Serial.print (",EX3:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[1][0]));
|
||||
Serial.print (",EX4:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[1][1]));
|
||||
Serial.print (",EX5:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[1][2]));
|
||||
Serial.print (",EX6:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[2][0]));
|
||||
Serial.print (",EX7:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
|
||||
Serial.print (",EX8:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
|
||||
Serial.print (",");
|
||||
#endif
|
||||
|
||||
#if PRINT_EULER == 1
|
||||
Serial.print("RLL:");
|
||||
Serial.print(ToDeg(roll));
|
||||
Serial.print(",PCH:");
|
||||
Serial.print(ToDeg(pitch));
|
||||
Serial.print(",YAW:");
|
||||
Serial.print(ToDeg(yaw));
|
||||
Serial.print(",IMUH:");
|
||||
Serial.print((imu_health>>8)&0xff);
|
||||
Serial.print (",");
|
||||
#endif
|
||||
|
||||
#if USE_MAGNETOMETER == 1
|
||||
Serial.print("MGX:");
|
||||
Serial.print(magnetom_x);
|
||||
Serial.print (",MGY:");
|
||||
Serial.print(magnetom_y);
|
||||
Serial.print (",MGZ:");
|
||||
Serial.print(magnetom_z);
|
||||
Serial.print (",MGH:");
|
||||
Serial.print(MAG_Heading);
|
||||
Serial.print (",");
|
||||
#endif
|
||||
|
||||
#if USE_BAROMETER == 1
|
||||
Serial.print("Temp:");
|
||||
Serial.print(temp_unfilt/20.0); // Convert into degrees C
|
||||
alti();
|
||||
Serial.print(",Pressure: ");
|
||||
Serial.print(press);
|
||||
// Serial.print(press>>2); // Convert into Pa
|
||||
Serial.print(",Alt: ");
|
||||
Serial.print(press_alt/1000); // Original floating point full solution in meters
|
||||
Serial.print (",");
|
||||
#endif
|
||||
|
||||
#if PRINT_GPS == 1
|
||||
if(gpsFixnew==1) {
|
||||
gpsFixnew=0;
|
||||
Serial.print("LAT:");
|
||||
Serial.print(lat);
|
||||
Serial.print(",LON:");
|
||||
Serial.print(lon);
|
||||
Serial.print(",ALT:");
|
||||
Serial.print(alt_MSL/1000); // meters
|
||||
Serial.print(",COG:");
|
||||
Serial.print((ground_course));
|
||||
Serial.print(",SOG:");
|
||||
Serial.print(ground_speed);
|
||||
Serial.print(",FIX:");
|
||||
Serial.print((int)gpsFix);
|
||||
Serial.print(",EVZ:"); //Vertical Speed
|
||||
Serial.print(ecefVZ);
|
||||
Serial.print(",SAT:");
|
||||
Serial.print((int)numSV);
|
||||
Serial.print (",");
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
gps_messages_sent++;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
//Serial.print("TOW:");
|
||||
//Serial.print(iTOW);
|
||||
//Serial.println("***");
|
||||
|
||||
#else
|
||||
// This section outputs binary data messages
|
||||
// Conforms to new binary message standard (12/31/09)
|
||||
byte IMU_buffer[22];
|
||||
int tempint;
|
||||
int ck;
|
||||
long templong;
|
||||
byte IMU_ck_a=0;
|
||||
byte IMU_ck_b=0;
|
||||
|
||||
// This section outputs the gps binary message when new gps data is available
|
||||
if(gpsFixnew==1) {
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
gps_messages_sent++;
|
||||
#endif
|
||||
gpsFixnew=0;
|
||||
Serial.print("DIYd"); // This is the message preamble
|
||||
IMU_buffer[0]=0x13;
|
||||
ck=19;
|
||||
IMU_buffer[1] = 0x03;
|
||||
|
||||
templong = lon; //Longitude *10**7 in 4 bytes
|
||||
IMU_buffer[2]=templong&0xff;
|
||||
IMU_buffer[3]=(templong>>8)&0xff;
|
||||
IMU_buffer[4]=(templong>>16)&0xff;
|
||||
IMU_buffer[5]=(templong>>24)&0xff;
|
||||
|
||||
templong = lat; //Latitude *10**7 in 4 bytes
|
||||
IMU_buffer[6]=templong&0xff;
|
||||
IMU_buffer[7]=(templong>>8)&0xff;
|
||||
IMU_buffer[8]=(templong>>16)&0xff;
|
||||
IMU_buffer[9]=(templong>>24)&0xff;
|
||||
|
||||
#if USE_BAROMETER==0
|
||||
tempint=alt_MSL / 100; // Altitude MSL in meters * 10 in 2 bytes
|
||||
#else
|
||||
alti();
|
||||
tempint = (press_alt * ALT_MIX + alt_MSL * (100-ALT_MIX)) / 10000; //Blended GPS and pressure altitude
|
||||
#endif
|
||||
IMU_buffer[10]=tempint&0xff;
|
||||
IMU_buffer[11]=(tempint>>8)&0xff;
|
||||
|
||||
tempint=speed_3d*100; // Speed in M/S * 100 in 2 bytes
|
||||
IMU_buffer[12]=tempint&0xff;
|
||||
IMU_buffer[13]=(tempint>>8)&0xff;
|
||||
|
||||
tempint=ground_course*100; // course in degreees * 100 in 2 bytes
|
||||
IMU_buffer[14]=tempint&0xff;
|
||||
IMU_buffer[15]=(tempint>>8)&0xff;
|
||||
|
||||
IMU_buffer[16]=iTOW&0xff;
|
||||
IMU_buffer[17]=(iTOW>>8)&0xff;
|
||||
IMU_buffer[18]=(iTOW>>16)&0xff;
|
||||
IMU_buffer[19]=(iTOW>>24)&0xff;
|
||||
|
||||
IMU_buffer[20]=(imu_health>>8)&0xff;
|
||||
|
||||
for (int i=0;i<ck+2;i++) Serial.print (IMU_buffer[i]);
|
||||
|
||||
for (int i=0;i<ck+2;i++) {
|
||||
IMU_ck_a+=IMU_buffer[i]; //Calculates checksums
|
||||
IMU_ck_b+=IMU_ck_a;
|
||||
}
|
||||
Serial.print(IMU_ck_a);
|
||||
Serial.print(IMU_ck_b);
|
||||
|
||||
} else {
|
||||
|
||||
// This section outputs the IMU orientatiom message
|
||||
Serial.print("DIYd"); // This is the message preamble
|
||||
IMU_buffer[0]=0x06;
|
||||
ck=6;
|
||||
IMU_buffer[1] = 0x02;
|
||||
|
||||
tempint=ToDeg(roll)*100; //Roll (degrees) * 100 in 2 bytes
|
||||
IMU_buffer[2]=tempint&0xff;
|
||||
IMU_buffer[3]=(tempint>>8)&0xff;
|
||||
|
||||
tempint=ToDeg(pitch)*100; //Pitch (degrees) * 100 in 2 bytes
|
||||
IMU_buffer[4]=tempint&0xff;
|
||||
IMU_buffer[5]=(tempint>>8)&0xff;
|
||||
|
||||
templong=(ToDeg(yaw)+gc_offset)*100; //Yaw (degrees) * 100 in 2 bytes
|
||||
if(templong>18000) templong -=36000;
|
||||
if(templong<-18000) templong +=36000;
|
||||
tempint = templong;
|
||||
IMU_buffer[6]=tempint&0xff;
|
||||
IMU_buffer[7]=(tempint>>8)&0xff;
|
||||
|
||||
for (int i=0;i<ck+2;i++) Serial.print (IMU_buffer[i]);
|
||||
|
||||
for (int i=0;i<ck+2;i++) {
|
||||
IMU_ck_a+=IMU_buffer[i]; //Calculates checksums
|
||||
IMU_ck_b+=IMU_ck_a;
|
||||
}
|
||||
Serial.print(IMU_ck_a);
|
||||
Serial.print(IMU_ck_b);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void printPerfData(long time)
|
||||
{
|
||||
|
||||
// This function outputs a performance monitoring message (used every 20 seconds)
|
||||
// Can be either binary or human readable
|
||||
#if PRINT_BINARY == 1
|
||||
byte IMU_buffer[30];
|
||||
int ck;
|
||||
byte IMU_ck_a=0;
|
||||
byte IMU_ck_b=0;
|
||||
|
||||
Serial.print("DIYd"); // This is the message preamble
|
||||
IMU_buffer[0]=0x11;
|
||||
ck=17;
|
||||
IMU_buffer[1] = 0x0a;
|
||||
|
||||
//Time for this reporting interval in millisecons
|
||||
IMU_buffer[2]=time&0xff;
|
||||
IMU_buffer[3]=(time>>8)&0xff;
|
||||
IMU_buffer[4]=(time>>16)&0xff;
|
||||
IMU_buffer[5]=(time>>24)&0xff;
|
||||
|
||||
IMU_buffer[6]=mainLoop_count&0xff;
|
||||
IMU_buffer[7]=(mainLoop_count>>8)&0xff;
|
||||
|
||||
IMU_buffer[8]=G_Dt_max&0xff;
|
||||
IMU_buffer[9]=(G_Dt_max>>8)&0xff;
|
||||
|
||||
IMU_buffer[10]=gyro_sat_count;
|
||||
IMU_buffer[11]=adc_constraints;
|
||||
IMU_buffer[12]=renorm_sqrt_count;
|
||||
IMU_buffer[13]=renorm_blowup_count;
|
||||
IMU_buffer[14]=gps_payload_error_count;
|
||||
IMU_buffer[15]=gps_checksum_error_count;
|
||||
IMU_buffer[16]=gps_pos_fix_count;
|
||||
IMU_buffer[17]=gps_nav_fix_count;
|
||||
IMU_buffer[18]=gps_messages_sent;
|
||||
|
||||
for (int i=0;i<ck+2;i++) Serial.print (IMU_buffer[i]);
|
||||
for (int i=0;i<ck+2;i++) {
|
||||
IMU_ck_a+=IMU_buffer[i]; //Calculates checksums
|
||||
IMU_ck_b+=IMU_ck_a;
|
||||
}
|
||||
Serial.print(IMU_ck_a);
|
||||
Serial.print(IMU_ck_b);
|
||||
|
||||
#else
|
||||
|
||||
/*
|
||||
Serial.print("PPP");
|
||||
Serial.print("pTm:");
|
||||
Serial.print(time,DEC);
|
||||
Serial.print(",mLc:");
|
||||
Serial.print(mainLoop_count,DEC);
|
||||
Serial.print(",DtM:");
|
||||
Serial.print(G_Dt_max,DEC);
|
||||
Serial.print(",gsc:");
|
||||
Serial.print(gyro_sat_count,DEC);
|
||||
Serial.print(",adc:");
|
||||
Serial.print(adc_constraints,DEC);
|
||||
Serial.print(",rsc:");
|
||||
Serial.print(renorm_sqrt_count,DEC);
|
||||
Serial.print(",rbc:");
|
||||
Serial.print(renorm_blowup_count,DEC);
|
||||
Serial.print(",gpe:");
|
||||
Serial.print(gps_payload_error_count,DEC);
|
||||
Serial.print(",gce:");
|
||||
Serial.print(gps_checksum_error_count,DEC);
|
||||
Serial.print(",gpf:");
|
||||
Serial.print(gps_pos_fix_count,DEC);
|
||||
Serial.print(",gnf:");
|
||||
Serial.print(gps_nav_fix_count,DEC);
|
||||
Serial.print(",gms:");
|
||||
Serial.print(gps_messages_sent,DEC);
|
||||
Serial.print(",imu:");
|
||||
Serial.print((imu_health>>8),DEC);
|
||||
Serial.print(",***");
|
||||
|
||||
*/
|
||||
#endif
|
||||
// Reset counters
|
||||
mainLoop_count = 0;
|
||||
G_Dt_max = 0;
|
||||
gyro_sat_count = 0;
|
||||
adc_constraints = 0;
|
||||
renorm_sqrt_count = 0;
|
||||
renorm_blowup_count = 0;
|
||||
gps_payload_error_count = 0;
|
||||
gps_checksum_error_count = 0;
|
||||
gps_pos_fix_count = 0;
|
||||
gps_nav_fix_count = 0;
|
||||
gps_messages_sent = 0;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
long convert_to_dec(float x)
|
||||
{
|
||||
return x*10000000;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,40 @@
|
||||
//Computes the dot product of two vectors
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
float op=0;
|
||||
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
op+=vector1[c]*vector2[c];
|
||||
}
|
||||
|
||||
return op;
|
||||
}
|
||||
|
||||
//Computes the cross product of two vectors
|
||||
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
//Multiply the vector by a scalar.
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn[c]*scale2;
|
||||
}
|
||||
}
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
+730
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,22 @@
|
||||
/**************************************************/
|
||||
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
|
||||
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=0;
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
|
||||
float test=mat[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,244 @@
|
||||
/* VTI SCP1000-D11 I2C pressure sensor */
|
||||
/* Based on code by Chris Barnes February 2010 */
|
||||
/* Based on code by Jose Julio */
|
||||
|
||||
/* ATTENTION: SCP1000 is a 3v3 device */
|
||||
|
||||
#if USE_BAROMETER == 1
|
||||
void setup_scp()
|
||||
{
|
||||
unsigned char temp_byte = 0;
|
||||
delay(6);
|
||||
// Init_Diagnostics(); // Initialise diagnostics output
|
||||
|
||||
// If we know the starting altitude we could initialise it
|
||||
// else just treat this as the altitude relative to the start
|
||||
// init_alt(START_ALTITUDE); // altitude in m ****************
|
||||
|
||||
|
||||
Wire.begin();
|
||||
TWIinit();
|
||||
SCP1000_StopAcquisition(); // Stop acquisition first - in case the sensor had already been put into a non-standby mode (e.g. Arduino s/w reset but sensor not)
|
||||
delay(100);
|
||||
SCP1000_StartAcquisition(); // SCP1000 only accepts new mode from standby - otherwise device will be left in previous mode
|
||||
/*
|
||||
Serial.print("Status: ");
|
||||
temp_byte = SCP1000_GetStatus2();
|
||||
Serial.println((int)temp_byte);
|
||||
DecodeStatus(temp_byte);
|
||||
|
||||
Serial.print("Operation Mode: ");
|
||||
Serial.println((int)SCP1000_GetOperation2());
|
||||
*/
|
||||
delay(100);
|
||||
}
|
||||
|
||||
/********************************************************************
|
||||
Altitude Calculation
|
||||
*********************************************************************/
|
||||
|
||||
void alti(void)
|
||||
{
|
||||
|
||||
double x;
|
||||
double p = (double)press_gnd/(double)press;
|
||||
double temp = (float)temperature/20.f + 273.15f;
|
||||
x = log(p) * temp * 29271.267f;
|
||||
//x = log(p) * temp * 29.271267 * 1000;
|
||||
press_alt = x + ground_alt;
|
||||
// Need to add comments for theory.....
|
||||
}
|
||||
|
||||
/********************************************************************
|
||||
Data Retrieval
|
||||
*********************************************************************/
|
||||
void SCP1000_StartAcquisition()
|
||||
{
|
||||
Wire.beginTransmission((uint8_t)PRESSURE_ADDR);
|
||||
Wire.send((uint8_t)SNS_ADDR_POPERATION); // Start acquisition
|
||||
Wire.send((uint8_t)SCP_MODE);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
void SCP1000_StopAcquisition()
|
||||
{
|
||||
Wire.beginTransmission((uint8_t)PRESSURE_ADDR);
|
||||
Wire.send((uint8_t)SNS_ADDR_POPERATION);
|
||||
Wire.send(0x00); // Stop acquisition
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
unsigned char SCP1000_GetStatus2()
|
||||
{
|
||||
unsigned char ready[] = {
|
||||
2, WRITE_PRESSURE_ADDR, SNS_ADDR_PSTATUS,
|
||||
2, READ_PRESSURE_ADDR, 0,
|
||||
0
|
||||
};
|
||||
TWIdocmd(ready);
|
||||
|
||||
return(ready[5]);
|
||||
}
|
||||
|
||||
unsigned char SCP1000_GetOperation2()
|
||||
{
|
||||
unsigned char oper[] = {
|
||||
2, WRITE_PRESSURE_ADDR, SNS_ADDR_POPERATION,
|
||||
2, READ_PRESSURE_ADDR, 0,
|
||||
0
|
||||
};
|
||||
TWIdocmd(oper);
|
||||
|
||||
return(oper[5]);
|
||||
}
|
||||
|
||||
// Return the raw temperature value provided by the SCP1000
|
||||
// This has units of degrees C x 20
|
||||
int SCP1000_GetTemp2()
|
||||
{
|
||||
int temp;
|
||||
|
||||
unsigned char getdatat[] = {
|
||||
2, WRITE_PRESSURE_ADDR, SNS_ADDR_PTEMP,
|
||||
3, READ_PRESSURE_ADDR, 0, 0,
|
||||
0
|
||||
};
|
||||
|
||||
TWIdocmd(getdatat);
|
||||
|
||||
// check sign bit of temperature
|
||||
if (0x20U & getdatat[5])
|
||||
{
|
||||
// negative temperature - extend sign
|
||||
getdatat[5] |= 0xC0;
|
||||
}
|
||||
temp = getdatat[5] << 8; // MSByte
|
||||
temp |= getdatat[6]; // LSByte
|
||||
|
||||
return(temp);
|
||||
}
|
||||
|
||||
// Return the raw pressure value provided by the SCP1000
|
||||
unsigned long SCP1000_GetPressure2()
|
||||
{
|
||||
unsigned long press;
|
||||
|
||||
unsigned char getdatap1[] = {
|
||||
2, WRITE_PRESSURE_ADDR, SNS_ADDR_DATARD8,
|
||||
2, READ_PRESSURE_ADDR, 0,
|
||||
0
|
||||
};
|
||||
unsigned char getdatap2[] = {
|
||||
2, WRITE_PRESSURE_ADDR, SNS_ADDR_PPRESSURE,
|
||||
3, READ_PRESSURE_ADDR, 0, 0,
|
||||
0
|
||||
};
|
||||
|
||||
TWIdocmd(getdatap1);
|
||||
|
||||
press = 0x07 & getdatap1[5]; // MSByte (either 5 or 6 for the sort of altitude we are interested in)
|
||||
|
||||
TWIdocmd(getdatap2);
|
||||
|
||||
press <<= 8;
|
||||
press |= getdatap2[5];
|
||||
press <<= 8;
|
||||
press |= getdatap2[6];
|
||||
|
||||
return(press);
|
||||
}
|
||||
|
||||
// Decode bits from status byte and return TRUE is there is a measurement ready to be read
|
||||
byte DecodeStatus(byte u8Status)
|
||||
{
|
||||
static byte u8Starting = 0U;
|
||||
|
||||
if (0U != (0x01 & u8Status))
|
||||
{
|
||||
//SCP1000 Startup procedure is still running
|
||||
if (u8Starting)
|
||||
{
|
||||
Serial.println("Starting");
|
||||
u8Starting = 1; // remember that we have already output this diagnostic
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
u8Starting = 0; // remember that we ahve seen the device out of starting mode
|
||||
if (0U != (0x10 & u8Status))
|
||||
{
|
||||
// Real Time Error - data was not read in time - clear by reading it
|
||||
// Note - this "error" is to be expected on startup when the SCP1000 is already running
|
||||
// as we will not have been reading out all of the measurements that have been made...
|
||||
Serial.println("RTErr");
|
||||
}
|
||||
if (0U != (0x20 & u8Status))
|
||||
{
|
||||
// DRDY - new data ready
|
||||
return (TRUE);
|
||||
}
|
||||
}
|
||||
return (FALSE);
|
||||
}
|
||||
|
||||
void ReadSCP1000(void)
|
||||
{
|
||||
temp_unfilt = SCP1000_GetTemp2();
|
||||
press = SCP1000_GetPressure2();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/********************************************************************
|
||||
This Library is needed for the SCP1000-D11
|
||||
because I2C commands needs a RESTART between commands and not and STOP-START (like Wire library do)
|
||||
This code is from http://harleyhacking.blogspot.com/
|
||||
*********************************************************************/
|
||||
|
||||
|
||||
#include <avr/io.h>
|
||||
|
||||
// TWI operations
|
||||
#define ENABTW ((1<<TWINT)|(1<<TWEN)|(0<<TWIE)) // 0x80 4 1
|
||||
#define START TWCR = (ENABTW|(1<<TWSTA)) // 0x20
|
||||
#define STOP TWCR = (ENABTW|(1<<TWSTO)) // 0x10
|
||||
#define SEND(x) TWDR = x; TWCR = ENABTW;
|
||||
#define RECV(ack) TWCR = ENABTW | (ack? (1<<TWEA) : 0 );
|
||||
unsigned char twista;
|
||||
unsigned twitmo;
|
||||
#define WAIT twitmo=0; while (!((twista = TWCR) & (1 << TWINT)) && ++twitmo);
|
||||
|
||||
/////===================================////////////////////
|
||||
void TWIinit(void)
|
||||
{
|
||||
DDRC &= ~0x30; // pullup
|
||||
PORTC |= 0x30; // pullup
|
||||
TWBR = (((F_CPU/400000)-16)/2); // 400 khz
|
||||
TWCR |= (1 << TWEN);
|
||||
}
|
||||
|
||||
void TWIdocmd(unsigned char *msg)
|
||||
{
|
||||
unsigned int mlen, rdwrf;
|
||||
|
||||
while ((mlen = *msg++)) {
|
||||
rdwrf = *msg & 1;
|
||||
START;
|
||||
WAIT;
|
||||
do {
|
||||
SEND(*msg++);
|
||||
WAIT;
|
||||
// should check for ACK - twista == SAWA or SDWA
|
||||
} while (--mlen && !rdwrf);
|
||||
// read
|
||||
while (mlen--) {
|
||||
RECV(mlen);
|
||||
WAIT;
|
||||
*msg++ = TWDR;
|
||||
}
|
||||
}
|
||||
STOP;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,23 @@
|
||||
extern unsigned long timer0_millis;
|
||||
|
||||
// this function replaces the arduino millis() funcion
|
||||
unsigned long DIYmillis()
|
||||
{
|
||||
unsigned long m;
|
||||
unsigned long m2;
|
||||
|
||||
// timer0_millis could change inside timer0 interrupt and we don´t want to disable interrupts
|
||||
// we can do two readings and compare.
|
||||
m = timer0_millis;
|
||||
m2 = timer0_millis;
|
||||
if (m!=m2) // timer0_millis corrupted?
|
||||
m = timer0_millis; // this should be fine...
|
||||
return m;
|
||||
}
|
||||
|
||||
void DIYdelay(unsigned long ms)
|
||||
{
|
||||
unsigned long start = DIYmillis();
|
||||
while (DIYmillis() - start <= ms)
|
||||
;
|
||||
}
|
||||
@@ -0,0 +1,212 @@
|
||||
/*
|
||||
C Datei für die Einbindung eines ArduIMU's
|
||||
Autoren@ZHAW: schmiemi
|
||||
chaneren
|
||||
*/
|
||||
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "modules/ins/ins_arduimu_modified.h"
|
||||
#include "firmwares/fixedwing/main_fbw.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
// test
|
||||
#include "estimator.h"
|
||||
|
||||
// für das Senden von GPS-Daten an den ArduIMU
|
||||
#include "gps.h"
|
||||
int32_t GPS_Data[14];
|
||||
|
||||
#ifndef ARDUIMU_I2C_DEV
|
||||
#define ARDUIMU_I2C_DEV i2c0
|
||||
#endif
|
||||
|
||||
// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
|
||||
// einzugebende Adresse im ArduIMU ist 0000 1011
|
||||
//da ArduIMU das Read/Write Bit selber anfügt.
|
||||
#define ArduIMU_SLAVE_ADDR 0x22
|
||||
|
||||
#ifndef DOWNLINK_DEVICE
|
||||
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||
#endif
|
||||
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "messages.h"
|
||||
#include "downlink.h"
|
||||
|
||||
struct i2c_transaction ardu_gps_trans;
|
||||
struct i2c_transaction ardu_ins_trans;
|
||||
|
||||
static int16_t recievedData[NB_DATA];
|
||||
float ArduIMU_data[NB_DATA];
|
||||
|
||||
float ins_roll_neutral;
|
||||
float ins_pitch_neutral;
|
||||
|
||||
void ArduIMU_init( void ) {
|
||||
ardu_ins_trans.status = I2CTransDone;
|
||||
ardu_gps_trans.status = I2CTransDone;
|
||||
|
||||
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
|
||||
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
|
||||
}
|
||||
|
||||
|
||||
#define GPS_DATA_MSG1 0
|
||||
#define GPS_DATA_MSG2 1
|
||||
|
||||
void ArduIMU_periodicGPS( void ) {
|
||||
static uint8_t gps_data_status = GPS_DATA_MSG1;
|
||||
|
||||
if (ardu_gps_trans.status != I2CTransDone) { return; }
|
||||
|
||||
if ( gps_data_status == GPS_DATA_MSG1 ) {
|
||||
//posllh
|
||||
GPS_Data [0] = gps_itow;
|
||||
GPS_Data [1] = gps_lon;
|
||||
GPS_Data [2] = gps_lat;
|
||||
GPS_Data [3] = gps_alt; //höhe über elipsoid
|
||||
GPS_Data [4] = gps_hmsl; //höhe über sea level
|
||||
//velned
|
||||
GPS_Data [5] = gps_speed_3d; //speed 3D
|
||||
GPS_Data [6] = gps_gspeed; //ground speed
|
||||
GPS_Data [7] = gps_course * 100000; //Kurs
|
||||
//status
|
||||
GPS_Data [8] = gps_mode; //fix
|
||||
GPS_Data [9] = gps_status_flags; //flags
|
||||
//sol
|
||||
GPS_Data [10] = gps_mode; //fix
|
||||
GPS_Data [11] = gps_sol_flags; //flags
|
||||
GPS_Data [12] = gps_ecefVZ; //ecefVZ
|
||||
GPS_Data [13] = gps_numSV;
|
||||
|
||||
//test für 32bit in byte packete abzupacken:
|
||||
//GPS_Data [0] = -1550138773;
|
||||
|
||||
ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed
|
||||
ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
|
||||
ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
|
||||
ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
|
||||
ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
|
||||
ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
|
||||
ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
|
||||
ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
|
||||
ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
|
||||
ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
|
||||
ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
|
||||
ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
|
||||
ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
|
||||
ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
|
||||
ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
|
||||
ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
|
||||
ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
|
||||
ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
|
||||
ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
|
||||
ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
|
||||
ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
|
||||
ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
|
||||
ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
|
||||
ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
|
||||
ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
|
||||
ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
|
||||
ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
|
||||
ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
|
||||
ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
|
||||
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28);
|
||||
|
||||
gps_data_status = GPS_DATA_MSG2;
|
||||
}
|
||||
else {
|
||||
|
||||
ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags
|
||||
ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
|
||||
ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
|
||||
ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
|
||||
ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
|
||||
ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
|
||||
ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
|
||||
ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
|
||||
ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
|
||||
ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
|
||||
ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
|
||||
ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
|
||||
ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
|
||||
ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
|
||||
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13);
|
||||
|
||||
gps_data_status = GPS_DATA_MSG1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ArduIMU_periodic( void ) {
|
||||
//Frequence defined in conf/modules/ins_arduimu.xml
|
||||
|
||||
if (ardu_ins_trans.status == I2CTransDone) {
|
||||
I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2);
|
||||
}
|
||||
|
||||
/*
|
||||
Buffer O: Roll
|
||||
Buffer 1: Pitch
|
||||
Buffer 2: Yaw
|
||||
Buffer 3: Gyro X
|
||||
Buffer 4: Gyro Y
|
||||
Buffer 5: Gyro Z
|
||||
Buffer 6: Accel X
|
||||
Buffer 7: Accel Y
|
||||
Buffer 8: Accel Z
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
void ArduIMU_event( void ) {
|
||||
// Handle INS I2C event
|
||||
if (ardu_ins_trans.status == I2CTransSuccess) {
|
||||
// received data from I2C transaction
|
||||
recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
|
||||
recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
|
||||
recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
|
||||
recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
|
||||
recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
|
||||
recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
|
||||
recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
|
||||
recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
|
||||
recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
|
||||
|
||||
// Update ArduIMU data
|
||||
ArduIMU_data[0] = ANGLE_FLOAT_OF_BFP(recievedData[0]);
|
||||
ArduIMU_data[1] = ANGLE_FLOAT_OF_BFP(recievedData[1]);
|
||||
ArduIMU_data[2] = ANGLE_FLOAT_OF_BFP(recievedData[2]);
|
||||
ArduIMU_data[3] = RATE_FLOAT_OF_BFP(recievedData[3]);
|
||||
ArduIMU_data[4] = RATE_FLOAT_OF_BFP(recievedData[4]);
|
||||
ArduIMU_data[5] = RATE_FLOAT_OF_BFP(recievedData[5]);
|
||||
ArduIMU_data[6] = ACCEL_FLOAT_OF_BFP(recievedData[6]);
|
||||
ArduIMU_data[7] = ACCEL_FLOAT_OF_BFP(recievedData[7]);
|
||||
ArduIMU_data[8] = ACCEL_FLOAT_OF_BFP(recievedData[8]);
|
||||
|
||||
// Update estimator
|
||||
estimator_phi = ArduIMU_data[0] - ins_roll_neutral;
|
||||
estimator_theta = ArduIMU_data[1] - ins_pitch_neutral;
|
||||
estimator_p = ArduIMU_data[3];
|
||||
//imu_daten_angefordert = FALSE;
|
||||
ardu_ins_trans.status = I2CTransDone;
|
||||
|
||||
{
|
||||
float psi=0;
|
||||
float ax=ArduIMU_data[6];
|
||||
RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &estimator_phi, &estimator_theta, &psi));
|
||||
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &ax, &(ArduIMU_data[7]), &(ArduIMU_data[8])));
|
||||
}
|
||||
}
|
||||
else if (ardu_ins_trans.status == I2CTransFailed) {
|
||||
ardu_ins_trans.status = I2CTransDone;
|
||||
}
|
||||
// Handle GPS I2C event
|
||||
if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
|
||||
ardu_gps_trans.status = I2CTransDone;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,22 @@
|
||||
#ifndef ArduIMU_H
|
||||
#define ArduIMU_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#define NB_DATA 9
|
||||
|
||||
extern float ArduIMU_data[NB_DATA];
|
||||
|
||||
extern float ins_roll_neutral;
|
||||
extern float ins_pitch_neutral;
|
||||
|
||||
//mixer
|
||||
extern float pitch_of_throttle_gain;
|
||||
extern float throttle_slew;
|
||||
|
||||
void ArduIMU_init( void );
|
||||
void ArduIMU_periodic( void );
|
||||
void ArduIMU_periodicGPS( void );
|
||||
void ArduIMU_event( void );
|
||||
|
||||
#endif // ArduIMU_H
|
||||
Reference in New Issue
Block a user