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; |
} |
} |
|
/* **************************************************************************** |