| 158,7 → 158,6 |
|---|
| void CalcHeading(void) |
| { |
| double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
| int16_t nick, roll; |
| int16_t heading = -1; |
| |
| // blink code for normal operation |
| 188,28 → 187,25 |
|---|
| switch(AttitudeSource) |
| { |
| case ATTITUDE_SOURCE_I2C: |
| cli(); // stop interrupts |
| nick = I2C_WriteAttitude.Nick; |
| roll = I2C_WriteAttitude.Roll; |
| sei(); // start interrupts |
| nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0); |
| roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0); |
| break; |
| |
| case ATTITUDE_SOURCE_UART: |
| cli(); // stop interrupts |
| nick = ExternData.Attitude[NICK]; |
| roll = ExternData.Attitude[ROLL]; |
| sei(); // start interrupts |
| nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0); |
| roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0); |
| break; |
| |
| case ATTITUDE_SOURCE_ACC: |
| nick = AccAttitudeNick; |
| roll = AccAttitudeRoll; |
| nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0); |
| roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0); |
| break; |
| |
| default: |
| nick_rad = 0; |
| roll_rad = 0; |
| break; |
| } |
| nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
| roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
| |
| // calculate attitude correction |
| Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
| 218,6 → 214,7 |
|---|
| DebugOut.Analog[27] = (int16_t)Hx; |
| DebugOut.Analog[28] = (int16_t)Hy; |
| |
| |
| // calculate Heading |
| heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
| // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
| 225,11 → 222,8 |
|---|
| if (heading < 0) heading = -heading; |
| else heading = 360 - heading; |
| |
| cli(); // stop interrupts |
| if(abs(heading) < 361) Heading = heading; |
| else (Heading = -1); |
| sei(); // start interrupts |
| |
| } |
| |
| |
| 239,7 → 233,6 |
|---|
| static uint8_t calold = 0; |
| static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
| static uint8_t blinkcount = 0; |
| static uint8_t invert_blinking = 0; |
| |
| // check both sources of communication for calibration request |
| if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
| 252,8 → 245,8 |
|---|
| { |
| if(CheckDelay(Led_Timer) || (cal != calold)) |
| { |
| if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF; |
| else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON; |
| if(blinkcount & 0x01) LED_GRN_OFF; |
| else LED_GRN_ON; |
| |
| // end of blinkcount sequence |
| if((blinkcount + 1 ) >= (2 * cal)) |
| 270,7 → 263,7 |
|---|
| } |
| else |
| { |
| if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF; |
| LED_GRN_OFF; |
| } |
| |
| // calibration state machine |
| 288,7 → 281,6 |
|---|
| Calibration.AccX.Offset = RawAccX; |
| Calibration.AccY.Offset = RawAccY; |
| Calibration.AccZ.Offset = RawAccZ; |
| invert_blinking = 0; |
| break; |
| |
| case 2: // 2nd step of calibration |
| 297,12 → 289,10 |
|---|
| if(UncalMagX > Xmax) Xmax = UncalMagX; |
| if(UncalMagY < Ymin) Ymin = UncalMagY; |
| if(UncalMagY > Ymax) Ymax = UncalMagY; |
| invert_blinking = 1; |
| break; |
| |
| case 3: // 3rd step of calibration |
| // used to change the orientation of the MK3MAG vertical to the horizontal plane |
| invert_blinking = 0; |
| break; |
| |
| case 4: |
| 309,7 → 299,6 |
|---|
| // find Min and Max of the Z-Sensor |
| if(UncalMagZ < Zmin) Zmin = UncalMagZ; |
| if(UncalMagZ > Zmax) Zmax = UncalMagZ; |
| invert_blinking = 1; |
| break; |
| |
| case 5: |
| 332,7 → 321,6 |
|---|
| blinkcount = 0; |
| } |
| } |
| invert_blinking = 0; |
| break; |
| |
| default: |