| /trunk/compass.c |
|---|
| 69,9 → 69,10 |
| volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation |
| volatile s16vec_t AccVector; // current acceleration vector of compass, not supported by any HW version |
| volatile s16vec_t MagVectorHorizontal; // vector componenents in horizontal projection |
| volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation |
| volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation |
| s16 Hx = 0, Hy = 0; |
| s32 EarthMagneticField = 100; |
| s32 EarthMagneticFieldFiltered = 100; |
| s32 EarthMagneticInclination = 0; |
| 148,7 → 149,6 |
| else // fc attitude is avialable and no compass calibration active |
| { |
| // calculate attitude correction |
| // a float implementation takes too long |
| s16 tmp; |
| s32 sinnick, cosnick, sinroll, cosroll; |
| tmp = FromFlightCtrl.AngleNick/10; // in deg |
| 157,11 → 157,10 |
| tmp = FromFlightCtrl.AngleRoll/10; // in deg |
| sinroll = (s32)c_sin_8192(tmp); |
| cosroll = (s32)c_cos_8192(tmp); |
| // tbd. compensation signs and oriantation has to be fixed |
| Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
| Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
| MagVectorHorizontal.X = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
| MagVectorHorizontal.Y = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
| // calculate heading |
| tmp = (s16)(c_atan2_546(Hy, Hx)/546L); |
| tmp = (s16)(c_atan2_546(MagVectorHorizontal.Y, MagVectorHorizontal.X)/546L); |
| if (tmp > 0) tmp = 360 - tmp; |
| else tmp = -tmp; |
| Compass_Heading = tmp; |
| /trunk/compass.h |
|---|
| 13,16 → 13,16 |
| extern volatile s16vec_t MagVector; // current magnetic field vector |
| extern volatile s16vec_t AccVector; // current acceleration vector of compass, not supported by any HW version |
| extern volatile s16vec_t MagVectorHorizontal; // vector componenents in horizontal projection |
| extern volatile s16 Compass_Heading; // current heading direction |
| extern volatile u8 Compass_CalState; // current calibration state |
| extern s32 EarthMagneticField; |
| extern s32 EarthMagneticFieldFiltered; |
| extern s32 EarthMagneticInclination; |
| extern s32 EarthMagneticInclinationFiltered; |
| extern s32 EarthMagneticInclinationTheoretic; |
| extern s32 EarthMagneticFieldFiltered; |
| extern u8 ErrorDisturbedEarthMagnetField; |
| extern s16 EarthMagneticStrengthTheoretic; |
| extern s16 Hx, Hy; |
| #define COMPASS_NONE 0 |
| #define COMPASS_MK3MAG 1 |
| /trunk/mk3mag.c |
|---|
| 300,17 → 300,17 |
| switch(Compass_CalState) |
| { |
| case 1: |
| if(last_state != Compass_CalState) |
| { |
| UART1_PutString("\r\nMK3Mag calibration\n\r"); |
| if(EarthMagneticStrengthTheoretic) |
| if(last_state != Compass_CalState) |
| { |
| UART1_PutString("\r\nMK3Mag calibration\n\r"); |
| if(EarthMagneticStrengthTheoretic) |
| { |
| MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50; |
| sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic); |
| UART1_PutString(msg); |
| } |
| else UART1_PutString("without GPS\n\r"); |
| } |
| MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50; |
| sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic); |
| UART1_PutString(msg); |
| } |
| else UART1_PutString("without GPS\n\r"); |
| } |
| x_max = -30000; y_max = -30000; z_max = -30000; |
| x_min = 30000; y_min = 30000; z_min = 30000; |
| speak = 1; |
| 336,18 → 336,18 |
| if(last_state == Compass_CalState) break; |
| if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration)) |
| { |
| BeepTime = 2500; |
| UART1_PutString("\r\n-> Calibration okay <-\n\r"); |
| SpeakHoTT = SPEAK_MIKROKOPTER; |
| BeepTime = 2500; |
| UART1_PutString("\r\n-> Calibration okay <-\n\r"); |
| SpeakHoTT = SPEAK_MIKROKOPTER; |
| } |
| else |
| { |
| UART1_PutString("\r\nCalibration FAILED - Values too low: "); |
| if((x_max - x_min) < MinCaclibration) UART1_PutString("X! "); |
| if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! "); |
| if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! "); |
| UART1_PutString("\r\n"); |
| SpeakHoTT = SPEAK_ERR_CALIBARTION; |
| UART1_PutString("\r\nCalibration FAILED - Values too low: "); |
| if((x_max - x_min) < MinCaclibration) UART1_PutString("X! "); |
| if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! "); |
| if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! "); |
| UART1_PutString("\r\n"); |
| SpeakHoTT = SPEAK_ERR_CALIBARTION; |
| } |
| UART1_PutString(msg); |
| sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min); |
| /trunk/mymath.c |
|---|
| 1,5 → 1,6 |
| #include "91x_lib.h" |
| #include "mymath.h" |
| #include <math.h> |
| // discrete mathematics |
| 35,7 → 36,7 |
| return (sinus * m * n); |
| } |
| s16 c_arccos2(s32 a,s32 b) |
| s16 c_arccos2(s32 a, s32 b) |
| { |
| if(a>b) return(0); |
| return(arccos64[64 * a / b]); |
| 150,4 → 151,3 |
| } |
| return(root >> 1); |
| } |