Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

fix course scaling in arduimu_basic

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-08-08 00:23:44 +02:00
parent db08f28539
commit c83237e5b5
4 changed files with 7 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
+2 -2
View File
@@ -117,8 +117,8 @@ void Drift_correction(void)
// Use GPS Ground course to correct yaw gyro drift
if(ground_speed>=SPEEDFILT)
{
COGX = cos(ToRad(ground_course));
COGY = sin(ToRad(ground_course));
COGX = cos(ground_course);
COGY = sin(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.
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
+3 -12
View File
@@ -20,9 +20,9 @@ void parse_pprz_gps() {
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
speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0; // Speed 3D (m/s) 0,1,2,3
ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D (m/s) 4,5,6,7
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/1e7; // Heading 2D (rad) 8,9,10,11
stGpsFix = Paparazzi_GPS_buffer[12];
calibrate_neutrals = Paparazzi_GPS_buffer[13];
high_accel_flag = Paparazzi_GPS_buffer[14];
@@ -52,12 +52,3 @@ int32_t join_4_bytes(byte Buffer[])
return(longUnion.dword);
}
/****************************************************************
*
void checksum(byte ubx_data)
{
ck_a+=ubx_data;
ck_b+=ck_a;
}
****************************************************************/
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
+1 -1
View File
@@ -152,7 +152,7 @@ union int_union {
int gpsFix=1; //This variable store the status of the GPS
float speed_3d=0; //Speed (3-D)
float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
float ground_course=90;//This is the runaway direction of you "plane" in degrees
float ground_course=0;//This is the runaway direction of you "plane" in radians
unsigned long GPS_timer=0;
//***********************GPS PAPARAZZI************************************************************************
sw/airborne/modules/ins/ins_arduimu_basic.c
+1 -1
View File
@@ -127,7 +127,7 @@ void ArduIMU_periodicGPS( void ) {
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)DegOfRad(gps.course / 1e6)); // course
FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps.course); // course
ardu_gps_trans.buf[12] = gps.fix; // status gps fix
ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals; // calibration flag
ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)