mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 06:15:43 +08:00
basic version of the arduimu firmware
returns angles rates and accels gps velocities and status are send no baro or magneto support
This commit is contained in:
Gautier Hattenberger
@@ -2,14 +2,14 @@
|
||||
|
||||
<module name="ins_ArduIMU" dir="ins">
|
||||
<header>
|
||||
<file name="ins_arduimu_modified.h"/>
|
||||
<file name="ins_arduimu_basic.h"/>
|
||||
</header>
|
||||
<init fun="ArduIMU_init()"/>
|
||||
<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_modified.c"/>
|
||||
<file name="ins_arduimu_basic.c"/>
|
||||
</makefile>
|
||||
<makefile target="sim">
|
||||
<file_arch name="ins_arduimu.c"/>
|
||||
|
||||
+51
-67
@@ -5,17 +5,17 @@ void Normalize(void)
|
||||
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
|
||||
@@ -36,7 +36,7 @@ void Normalize(void)
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:1,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
@@ -44,8 +44,8 @@ void Normalize(void)
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
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
|
||||
@@ -75,7 +75,7 @@ void Normalize(void)
|
||||
#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
|
||||
@@ -103,18 +103,18 @@ void Normalize(void)
|
||||
#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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -122,15 +122,13 @@ void Normalize(void)
|
||||
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
|
||||
@@ -139,48 +137,35 @@ void Drift_correction(void)
|
||||
// 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
|
||||
|
||||
|
||||
#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
|
||||
|
||||
// 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)) {
|
||||
@@ -192,14 +177,13 @@ void Drift_correction(void)
|
||||
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
|
||||
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
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
@@ -208,17 +192,17 @@ 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
|
||||
|
||||
#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
|
||||
@@ -228,7 +212,7 @@ void Matrix_update(void)
|
||||
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)
|
||||
#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
|
||||
@@ -238,7 +222,7 @@ void Matrix_update(void)
|
||||
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
|
||||
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
@@ -253,14 +237,14 @@ void Matrix_update(void)
|
||||
|
||||
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
|
||||
#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,64 @@
|
||||
/****************************************************************
|
||||
* Parse GPS data from a Paparazzi autopilot
|
||||
****************************************************************/
|
||||
// Code from Jordi, modified by Jose, modified by Gautier
|
||||
|
||||
|
||||
void init_gps(void)
|
||||
{
|
||||
//Serial.begin(38400);
|
||||
pinMode(2,OUTPUT); //Serial Mux
|
||||
digitalWrite(2,HIGH); //Serial Mux
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
void parse_pprz_gps() {
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
gps_pos_fix_count++;
|
||||
#endif
|
||||
|
||||
speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0; // m/s 0,1,2,3
|
||||
ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D 4,5,6,7
|
||||
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11
|
||||
stGpsFix = Paparazzi_GPS_buffer[12];
|
||||
stFlags = Paparazzi_GPS_buffer[13];
|
||||
|
||||
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 (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
* 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;
|
||||
}
|
||||
|
||||
****************************************************************/
|
||||
@@ -0,0 +1,200 @@
|
||||
|
||||
//*****I2C Output************************************************************
|
||||
|
||||
void requestEvent(){
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.println("Sending IMU Data");
|
||||
#endif
|
||||
|
||||
// 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((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
|
||||
|
||||
byte* pointer;
|
||||
pointer = (byte*) &I2C_Message_ar;
|
||||
Wire.send(pointer, 18);
|
||||
|
||||
}
|
||||
|
||||
//******** GPS Data from Paparazzi AP ******************************************
|
||||
|
||||
void receiveEvent(int howMany){
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("Receiving GPS Bytes : ");
|
||||
Serial.println(howMany);
|
||||
#endif
|
||||
|
||||
for(int i=0; i < howMany; i++){
|
||||
Paparazzi_GPS_buffer[i]=Wire.receive();
|
||||
}
|
||||
|
||||
parse_pprz_gps(); // Parse new GPS packet...
|
||||
GPS_timer=DIYmillis(); //Restarting timer...
|
||||
|
||||
gpsDataReady=1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
//*************************************************************************************************************
|
||||
|
||||
|
||||
void printdata(void){
|
||||
|
||||
#if PRINT_I2C_MSG == 1
|
||||
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
|
||||
|
||||
//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 PRINT_GPS == 1
|
||||
if(gpsFixnew==1) {
|
||||
gpsFixnew=0;
|
||||
Serial.print("COG:");
|
||||
Serial.print((ground_course));
|
||||
Serial.print(",SOG:");
|
||||
Serial.print(ground_speed);
|
||||
Serial.print(",FIX:");
|
||||
Serial.print((int)gpsFix);
|
||||
Serial.print (",");
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
gps_messages_sent++;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void printPerfData(long time)
|
||||
{
|
||||
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(",***");
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
+194
-357
File diff suppressed because it is too large
Load Diff
@@ -1,74 +0,0 @@
|
||||
/* ******************************************************* */
|
||||
/* 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);
|
||||
}
|
||||
@@ -1,120 +0,0 @@
|
||||
#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
|
||||
|
||||
speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0; // m/s 0,1,2,3
|
||||
ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D 4,5,6,7
|
||||
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11
|
||||
alt = join_4_bytes(&Paparazzi_GPS_buffer[12]); // 12,13,14,15
|
||||
alt_MSL = join_4_bytes(&Paparazzi_GPS_buffer[16]); // 16,17,18,19
|
||||
stGpsFix = Paparazzi_GPS_buffer[20];
|
||||
stFlags = Paparazzi_GPS_buffer[21];
|
||||
|
||||
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 (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
|
||||
|
||||
#if 0
|
||||
// Serial.print("Time von Arduino ;");
|
||||
// Serial.print(millis());
|
||||
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);
|
||||
#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
|
||||
@@ -1,419 +0,0 @@
|
||||
|
||||
|
||||
//*****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((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
|
||||
|
||||
byte* pointer;
|
||||
pointer = (byte*) &I2C_Message_ar;
|
||||
Wire.send(pointer, 18);
|
||||
|
||||
/* 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((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(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(",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 (",");
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
gps_messages_sent++;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#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]=0;
|
||||
IMU_buffer[17]=0;
|
||||
IMU_buffer[18]=0;
|
||||
IMU_buffer[19]=0;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -1,244 +0,0 @@
|
||||
/* 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
|
||||
+19
-34
@@ -6,8 +6,8 @@ Autoren@ZHAW: schmiemi
|
||||
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "modules/ins/ins_arduimu_modified.h"
|
||||
#include "firmwares/fixedwing/main_fbw.h"
|
||||
#include "modules/ins/ins_arduimu_basic.h"
|
||||
//#include "firmwares/fixedwing/main_fbw.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
// test
|
||||
@@ -15,7 +15,6 @@ Autoren@ZHAW: schmiemi
|
||||
|
||||
// für das Senden von GPS-Daten an den ArduIMU
|
||||
#include "gps.h"
|
||||
int32_t GPS_Data[10];
|
||||
|
||||
#define NB_DATA 9
|
||||
|
||||
@@ -71,25 +70,12 @@ void ArduIMU_periodicGPS( void ) {
|
||||
|
||||
if (ardu_gps_trans.status != I2CTransDone) { return; }
|
||||
|
||||
//velned
|
||||
GPS_Data [0] = gps_speed_3d; //speed 3D
|
||||
GPS_Data [1] = gps_gspeed; //ground speed
|
||||
GPS_Data [2] = gps_course * 100000; //Kurs
|
||||
//alt
|
||||
GPS_Data [3] = gps_alt; // height above elipsoid
|
||||
GPS_Data [4] = gps_hmsl; // height above sea level
|
||||
//status
|
||||
GPS_Data [5] = gps_mode; //fix
|
||||
GPS_Data [6] = gps_status_flags; //flags
|
||||
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 0, GPS_Data[0]);
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 4, GPS_Data[1]);
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 8, GPS_Data[2]);
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 12, GPS_Data[3]);
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 16, GPS_Data[4]);
|
||||
ardu_gps_trans.buf[20] = GPS_Data[5]; // status gps fix
|
||||
ardu_gps_trans.buf[21] = GPS_Data[6]; // status flags
|
||||
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 22);
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed
|
||||
FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course
|
||||
ardu_gps_trans.buf[12] = gps_mode; // status gps fix
|
||||
ardu_gps_trans.buf[13] = gps_status_flags; // status flags
|
||||
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
|
||||
|
||||
}
|
||||
|
||||
@@ -100,21 +86,20 @@ void ArduIMU_periodic( void ) {
|
||||
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"
|
||||
/*
|
||||
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
|
||||
*/
|
||||
|
||||
void ArduIMU_event( void ) {
|
||||
// Handle INS I2C event
|
||||
Reference in New Issue
Block a user