| 114,7 → 114,6 |
|---|
| } |
| |
| |
| |
| /* **************************************************************************** |
| Functionname: IMU_Main */ /*! |
| Description: |
| 182,7 → 181,7 |
|---|
| /* Check the Batery for Undervoltage */ |
| TestBattery(); |
| /* DeployRescue */ |
| /* DeployRescue(); */ |
| DeployRescue(); |
| } |
| } |
| |
| 197,18 → 196,26 |
|---|
| **************************************************************************** */ |
| void DeployRescue() |
| { |
| #if 0 |
| /* Yaw or pitch are greater than 60 Deg abs */ |
| if (((abs(status.iTheta10) > 600) || (abs(status.iPhi10) > 600)) && |
| ((abs(AverageRoll) > Threshhold) || |
| (abs(AverageNick) > Threshhold) || |
| (abs(AverageGier) > Threshhold) )) |
| static int InvalidAttitude = 0; |
| /* Yaw or pitch are greater than 90 Deg abs */ |
| if (((abs(status.iTheta10) > 900) || (abs(status.iPhi10) > 900)) && |
| ((abs(AverageRoll) > 400) || |
| (abs(AverageNick) > 400) || |
| (abs(AverageGier) > 400) )) |
| { |
| MotorenEin = 0; |
| Delay_ms(1000); |
| ReleaseServo(); |
| //MotorenEin = 0; |
| Delay_ms(500); |
| InvalidAttitude = 1; |
| } |
| #endif |
| |
| if (InvalidAttitude) |
| { |
| ServoValue = 150; |
| } |
| else |
| { |
| ServoValue = 0; |
| } |
| } |
| |
| /* **************************************************************************** |