Rev 838 | Rev 970 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 838 | Rev 966 | ||
---|---|---|---|
Line 1... | Line -... | ||
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
- | 1 | /* |
|
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | Copyright 2008, by Michael Walter |
3 | // + Nur für den privaten Gebrauch |
- | |
4 | // + www.MikroKopter.com |
- | |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
- | |
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
- | |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
- | |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
- | |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
- | |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
- | |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
- | |
- | 3 | ||
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
- | |
17 | // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
- | |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
- | |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
- | |
21 | // + Benutzung auf eigene Gefahr |
- | |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
- | |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
25 | // + mit unserer Zustimmung zulässig |
- | |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
- | |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
30 | // + this list of conditions and the following disclaimer. |
- | |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
32 | // + from this software without specific prior written permission. |
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
- | 9 | ||
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
34 | // + for non-commercial use (directly or indirectly) |
- | |
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
36 | // + with our written permission |
12 | unless it is stated otherwise. |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
- | |
38 | // + clearly linked as origin |
- | |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
- | |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
- | |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
- | |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
- | |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
- | |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
- | |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
- | |
- | 13 | */ |
|
- | 14 | ||
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
15 | /***************************************************************************** |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
- | |
48 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
- | |
49 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
- | |
50 | // + POSSIBILITY OF SUCH DAMAGE. |
16 | INCLUDES |
51 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | **************************************************************************** */ |
52 | #include "main.h" |
18 | #include "main.h" |
53 | #include "kafi.h" |
19 | #include "kafi.h" |
54 | #include "mymath.h" |
20 | #include "mymath.h" |
55 | #include "mm3.h" |
21 | #include "mm3.h" |
56 | 22 | ||
- | 23 | /***************************************************************************** |
|
- | 24 | (SYMBOLIC) CONSTANTS |
|
- | 25 | *****************************************************************************/ |
|
- | 26 | ||
57 | #define sin45 -0.707106 |
27 | #define sin45 -0.707106 |
58 | #define cos45 0.707106 |
28 | #define cos45 0.707106 |
Line -... | Line 29... | ||
- | 29 | ||
- | 30 | /***************************************************************************** |
|
- | 31 | VARIABLES |
|
- | 32 | *****************************************************************************/ |
|
- | 33 | extern int RCQuality; |
|
- | 34 | int RemoteLinkLost; |
|
59 | 35 | extern unsigned long maxDistance; |
|
- | 36 | unsigned char EEPromArray[E2END+1] EEMEM; |
|
- | 37 | ||
- | 38 | void TestRemote(void); |
|
- | 39 | void IMU_Main(void); |
|
- | 40 | void TestBattery(void); |
|
- | 41 | void SendDebugData(void); |
|
- | 42 | void InitPorts(void); |
|
- | 43 | void GenerateDefaults(void); |
|
- | 44 | void TestIC2Link(void); |
|
- | 45 | void GetAirPressureOffset(void); |
|
- | 46 | void DeployRescue(void); |
|
60 | unsigned char EEPromArray[E2END+1] EEMEM; |
47 | |
61 | extern void InitOSD(void); |
48 | extern void InitOSD(void); |
62 | extern void InitGPS(void); |
- | |
63 | 49 | extern void InitGPS(void); |
|
64 | extern void SendOSD(void); |
50 | extern void SendOSD(void); |
65 | extern void MotorControl(void); |
51 | extern void RemoteControl(void); |
66 | extern void SendMotorData(void); |
52 | extern void SendMotorData(void); |
67 | extern void CalculateAverage(void); |
53 | extern void GetMeasurements(void); |
68 | extern void AttitudeEstimation(void); |
54 | extern void AttitudeEstimation(void); |
69 | extern void PID_Regler(void); |
55 | extern void PD_Regler(void); |
Line 70... | Line 56... | ||
70 | extern void SetNeutral(void); |
56 | extern void SetNeutral(void); |
- | 57 | ||
- | 58 | /* **************************************************************************** |
|
- | 59 | Functionname: main */ /*! |
|
- | 60 | Description: Hauptprogramm |
|
- | 61 | ||
- | 62 | @return void |
|
- | 63 | @pre - |
|
- | 64 | @post - |
|
- | 65 | @author Michael Walter |
|
- | 66 | **************************************************************************** */ |
|
- | 67 | int main (void) |
|
- | 68 | { |
|
- | 69 | /* Controler Init */ |
|
- | 70 | InitPorts(); |
|
- | 71 | Kafi_Init(); |
|
- | 72 | Timer_Init(); |
|
- | 73 | UART_Init(); |
|
- | 74 | InitGPS(); |
|
- | 75 | rc_sum_init(); |
|
- | 76 | ADC_Init(); |
|
- | 77 | i2c_init(); |
|
- | 78 | #ifdef USE_COMPASS |
|
- | 79 | MM3_Init(); |
|
- | 80 | #endif |
|
- | 81 | /* Enable Interrupts */ |
|
- | 82 | sei(); |
|
- | 83 | ||
- | 84 | /* Generate Default Values */ |
|
- | 85 | GenerateDefaults (); |
|
- | 86 | /* Get Air Pressure Offset */ |
|
- | 87 | GetAirPressureOffset(); |
|
- | 88 | /* Determine Gyro / ACC Offsets */ |
|
- | 89 | SetNeutral(); |
|
- | 90 | ||
- | 91 | DebugIn.Analog[1] = 1000; |
|
- | 92 | DebugIn.Digital[0] = 0x55; |
|
- | 93 | ||
- | 94 | #ifdef USE_COMPASS |
|
- | 95 | /* Calibrate the MM3 Compass? */ |
|
- | 96 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && |
|
- | 97 | (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && |
|
- | 98 | (MotorenEin == 0)) |
|
- | 99 | { |
|
- | 100 | printf("\n\rCalibrating Compass"); |
|
- | 101 | MM3_Calibrate(); |
|
- | 102 | } |
|
- | 103 | #endif |
|
- | 104 | ||
- | 105 | #ifdef USE_OSD |
|
- | 106 | /* Init the Bob-4 OnScreen Display */ |
|
- | 107 | InitOSD(); |
|
- | 108 | #endif |
|
- | 109 | ||
- | 110 | /* Start the main Task */ |
|
- | 111 | IMU_Main(); |
|
- | 112 | return (1); |
|
- | 113 | } |
|
- | 114 | ||
- | 115 | ||
- | 116 | ||
- | 117 | /* **************************************************************************** |
|
- | 118 | Functionname: IMU_Main */ /*! |
|
- | 119 | Description: |
|
- | 120 | ||
- | 121 | @param[in] |
|
- | 122 | ||
- | 123 | @return void |
|
- | 124 | @pre - |
|
- | 125 | @post - |
|
- | 126 | @author Michael Walter |
|
- | 127 | **************************************************************************** */ |
|
- | 128 | void IMU_Main() |
|
- | 129 | { |
|
- | 130 | ROT_ON |
|
- | 131 | I2CTimeout = 5000; |
|
- | 132 | ||
- | 133 | while (1) |
|
- | 134 | { |
|
- | 135 | static i32_t OldTime = 0; |
|
- | 136 | if (UpdateMotor) |
|
- | 137 | { |
|
- | 138 | UpdateMotor=0; |
|
- | 139 | ||
- | 140 | #ifdef USE_OSD |
|
- | 141 | /* G e n e r a t e O S D D a t a */ |
|
- | 142 | static char CntOSD = 0; |
|
- | 143 | if (CntOSD % 6 == 1) |
|
- | 144 | { |
|
- | 145 | SendOSD(); |
|
- | 146 | } |
|
- | 147 | CntOSD++; |
|
- | 148 | #endif |
|
- | 149 | ||
- | 150 | /* Set the cycle Time to 120ms / 125ms */ |
|
- | 151 | if (OldTime != 0) |
|
- | 152 | { |
|
- | 153 | while (((Count8Khz - OldTime) *10) / 8 < 120); |
|
- | 154 | DebugOut.Analog[14] = ((Count8Khz - OldTime) *10) / 8; |
|
- | 155 | } |
|
- | 156 | OldTime = Count8Khz; |
|
- | 157 | ||
- | 158 | /*GetMeasurements()*/ |
|
- | 159 | GetMeasurements(); |
|
- | 160 | /*EstimateFlightAttitude */ |
|
- | 161 | FlightAttitudeEstimation(); |
|
- | 162 | /* Set Nominal Value */ |
|
- | 163 | RemoteControl(); |
|
- | 164 | /* PID Control */ |
|
- | 165 | PD_Regler(); |
|
- | 166 | /* Send Motor Data */ |
|
- | 167 | SendMotorData(); |
|
- | 168 | /* TestRemote */ |
|
- | 169 | TestRemote(); |
|
- | 170 | /* Test IC2- / RC-Link */ |
|
- | 171 | TestIC2Link(); |
|
- | 172 | } |
|
- | 173 | /* Send Debug Data over RS232 */ |
|
- | 174 | SendDebugData(); |
|
- | 175 | /* Check the Batery for Undervoltage */ |
|
- | 176 | TestBattery(); |
|
- | 177 | /* DeployRescue */ |
|
- | 178 | //DeployRescue(); |
|
- | 179 | } |
|
- | 180 | } |
|
- | 181 | ||
- | 182 | /* **************************************************************************** |
|
- | 183 | Functionname: DeployRescue */ /*! |
|
- | 184 | Description: Deploy a rescue parachute using a servo |
|
- | 185 | ||
- | 186 | @return void |
|
- | 187 | @pre - |
|
- | 188 | @post - |
|
- | 189 | @author Michael Walter |
|
- | 190 | **************************************************************************** */ |
|
- | 191 | void DeployRescue() |
|
- | 192 | { |
|
- | 193 | #if 0 |
|
- | 194 | /* Yaw or pitch are greater than 60 Deg abs */ |
|
- | 195 | if (((abs(status.iTheta10) > 600) || (abs(status.iPhi10) > 600)) && |
|
- | 196 | ((abs(AverageRoll_X) > Threshhold) || |
|
- | 197 | (abs(AverageRoll_Y) > Threshhold) || |
|
- | 198 | (abs(AverageRoll_Z) > Threshhold) )) |
|
- | 199 | { |
|
- | 200 | MotorenEin = 0; |
|
- | 201 | Delay_ms(1000); |
|
- | 202 | ReleaseServo(); |
|
- | 203 | } |
|
- | 204 | #endif |
|
- | 205 | } |
|
71 | 206 | ||
72 | /* **************************************************************************** |
207 | /* **************************************************************************** |
73 | Functionname: ReadParameterSet */ /*! |
208 | Functionname: ReadParameterSet */ /*! |
Line 74... | Line 209... | ||
74 | Description: -- Parametersatz aus EEPROM lesen --- |
209 | Description: -- Parametersatz aus EEPROM lesen --- |
Line 79... | Line 214... | ||
79 | @post - |
214 | @post - |
80 | @author H. Buss / I. Busker |
215 | @author H. Buss / I. Busker |
81 | **************************************************************************** */ |
216 | **************************************************************************** */ |
82 | void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
217 | void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
83 | { |
218 | { |
84 | if (number > 5) |
219 | if (number > 5) |
85 | { |
220 | { |
86 | number = 5; |
221 | number = 5; |
87 | } |
222 | } |
88 | eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
223 | eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
89 | } |
224 | } |
Line 90... | Line 225... | ||
90 | 225 | ||
91 | /* **************************************************************************** |
226 | /* **************************************************************************** |
92 | Functionname: WriteParameterSet */ /*! |
227 | Functionname: WriteParameterSet */ /*! |
Line 98... | Line 233... | ||
98 | @post - |
233 | @post - |
99 | @author H. Buss / I. Busker |
234 | @author H. Buss / I. Busker |
100 | **************************************************************************** */ |
235 | **************************************************************************** */ |
101 | void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
236 | void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
102 | { |
237 | { |
103 | if(number > 5) |
238 | if(number > 5) |
104 | { |
239 | { |
105 | number = 5; |
240 | number = 5; |
106 | } |
241 | } |
107 | eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
242 | eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
108 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
243 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
109 | } |
244 | } |
Line 110... | Line -... | ||
110 | - | ||
111 | 245 | ||
112 | /* **************************************************************************** |
246 | /* **************************************************************************** |
113 | Functionname: GetActiveParamSetNumber */ /*! |
247 | Functionname: GetActiveParamSetNumber */ /*! |
Line 114... | Line 248... | ||
114 | Description: |
248 | Description: |
Line 118... | Line 252... | ||
118 | @post - |
252 | @post - |
119 | @author H. Buss / I. Busker |
253 | @author H. Buss / I. Busker |
120 | **************************************************************************** */ |
254 | **************************************************************************** */ |
121 | unsigned char GetActiveParamSetNumber(void) |
255 | unsigned char GetActiveParamSetNumber(void) |
122 | { |
256 | { |
123 | unsigned char set; |
257 | unsigned char set; |
124 | set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
258 | set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
125 | if(set > 5) |
259 | if(set > 5) |
126 | { |
260 | { |
127 | set = 2; |
261 | set = 2; |
128 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
262 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
129 | } |
263 | } |
130 | return(set); |
264 | return(set); |
131 | } |
265 | } |
Line -... | Line 266... | ||
- | 266 | ||
- | 267 | /* **************************************************************************** |
|
- | 268 | Functionname: GetAirPressureOffset */ /*! |
|
- | 269 | Description: |
|
- | 270 | ||
- | 271 | @return void |
|
- | 272 | @pre - |
|
- | 273 | @post - |
|
- | 274 | @author H. Buss / I. Busker |
|
- | 275 | **************************************************************************** */ |
|
- | 276 | void GetAirPressureOffset(void) |
|
- | 277 | { |
|
- | 278 | unsigned int timer; |
|
- | 279 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
|
- | 280 | printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber()); |
|
- | 281 | if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
|
- | 282 | { |
|
- | 283 | printf("\n\rAbgleich Luftdrucksensor.."); |
|
- | 284 | timer = SetDelay(1000); |
|
- | 285 | SucheLuftruckOffset(); |
|
- | 286 | while (!CheckDelay(timer)); |
|
- | 287 | printf("OK\n\r"); |
|
- | 288 | } |
|
Line 132... | Line 289... | ||
132 | 289 | } |
|
133 | 290 | ||
134 | /* **************************************************************************** |
291 | /* **************************************************************************** |
Line 135... | Line 292... | ||
135 | Functionname: main */ /*! |
292 | Functionname: GenerateDefaults */ /*! |
136 | Description: Hauptprogramm |
293 | Description: Generate the Default Paramter |
137 | 294 | ||
138 | @return void |
295 | @return void |
139 | @pre - |
296 | @pre - |
140 | @post - |
297 | @post - |
141 | @author H. Buss / I. Busker |
298 | @author H. Buss / I. Busker |
319 | } |
384 | { |
Line -... | Line 385... | ||
- | 385 | I2CTimeout--; |
|
- | 386 | } |
|
- | 387 | } |
|
- | 388 | ||
- | 389 | ||
- | 390 | /* **************************************************************************** |
|
- | 391 | Functionname: SendDebugData */ /*! |
|
- | 392 | Description: Send Debug Data over RS232 |
|
- | 393 | ||
- | 394 | @return void |
|
- | 395 | @pre - |
|
- | 396 | @post - |
|
- | 397 | @author H. Buss / I. Busker |
|
- | 398 | **************************************************************************** */ |
|
- | 399 | void SendDebugData() |
|
- | 400 | { |
|
- | 401 | if(SIO_DEBUG) |
|
- | 402 | { |
|
- | 403 | DatenUebertragung(); |
|
- | 404 | BearbeiteRxDaten(); |
|
- | 405 | } |
|
- | 406 | else |
|
- | 407 | { |
|
- | 408 | BearbeiteRxDaten(); |
|
- | 409 | } |
|
- | 410 | } |
|
- | 411 | ||
- | 412 | ||
- | 413 | /* **************************************************************************** |
|
- | 414 | Functionname: TestBattery */ /*! |
|
- | 415 | Description: Check the Battery Voltage |
|
- | 416 | ||
- | 417 | @return void |
|
- | 418 | @pre - |
|
- | 419 | @post - |
|
- | 420 | @author H. Buss / I. Busker |
|
- | 421 | **************************************************************************** */ |
|
- | 422 | void TestBattery() |
|
- | 423 | { |
|
- | 424 | unsigned int timer = 0; |
|
- | 425 | if(CheckDelay(timer)) |
|
- | 426 | { /* Voltage is Below Threshhod ? */ |
|
- | 427 | if(UBat < EE_Parameter.UnterspannungsWarnung) |
|
- | 428 | { |
|
- | 429 | if(BeepMuster == 0xffff) |
|
- | 430 | { |
|
- | 431 | beeptime = 6000; |
|
- | 432 | BeepMuster = 0x0300; |
|
- | 433 | } |
|
- | 434 | } |
|
- | 435 | timer = SetDelay(100); |
|
- | 436 | } |
|
- | 437 | DebugOut.Analog[15] = UBat; |
|
- | 438 | } |
|
- | 439 | ||
- | 440 | /* **************************************************************************** |
|
- | 441 | Functionname: TestRemote */ /*! |
|
- | 442 | Description: |
|
- | 443 | ||
- | 444 | @param[in] |
|
- | 445 | ||
- | 446 | @return void |
|
- | 447 | @pre - |
|
- | 448 | @post - |
|
- | 449 | @author Michael Walter |
|
- | 450 | **************************************************************************** */ |
|
- | 451 | void TestRemote() |
|
- | 452 | { |
|
- | 453 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
|
- | 454 | ||
- | 455 | /*--- VARIABLES ---*/ |
|
- | 456 | static unsigned int TimeSinceRC_BelowThreshhold = 0; |
|
- | 457 | static unsigned int TimeSinceRC_AboveThreshhold = 0; |
|
- | 458 | ||
- | 459 | if (MotorenEin == 1) |
|
- | 460 | { |
|
- | 461 | if (RemoteLinkLost == 0) |
|
- | 462 | { |
|
- | 463 | if (RCQuality < 60) |
|
- | 464 | { |
|
- | 465 | if (TimeSinceRC_BelowThreshhold < 10000) |
|
- | 466 | { |
|
- | 467 | TimeSinceRC_BelowThreshhold++; |
|
- | 468 | } |
|
- | 469 | } |
|
- | 470 | else |
|
- | 471 | { |
|
- | 472 | TimeSinceRC_BelowThreshhold = 0; |
|
- | 473 | } |
|
- | 474 | if ((TimeSinceRC_BelowThreshhold > 500) || /* aprox. 5 seconds */ |
|
- | 475 | (SenderOkay < 100)) |
|
- | 476 | { |
|
- | 477 | RemoteLinkLost = 1; |
|
- | 478 | TimeSinceRC_AboveThreshhold = 0; |
|
- | 479 | } |
|
- | 480 | } |
|
- | 481 | else |
|
- | 482 | { |
|
- | 483 | if (RCQuality > 80) |
|
- | 484 | { |
|
- | 485 | if (TimeSinceRC_AboveThreshhold < 10000) |
|
- | 486 | { |
|
- | 487 | TimeSinceRC_AboveThreshhold++; |
|
- | 488 | } |
|
- | 489 | } |
|
- | 490 | else |
|
- | 491 | { |
|
- | 492 | TimeSinceRC_AboveThreshhold = 0; |
|
- | 493 | } |
|
- | 494 | if (TimeSinceRC_AboveThreshhold > 100) /* aprox. 1 seconds */ |
|
- | 495 | { |
|
- | 496 | RemoteLinkLost = 0; |
|
- | 497 | TimeSinceRC_BelowThreshhold = 0; |
|
- | 498 | } |
|
- | 499 | } |
|
- | 500 | } |
|
- | 501 | else |
|
- | 502 | { |
|
- | 503 | RemoteLinkLost = 0; |
|
- | 504 | TimeSinceRC_BelowThreshhold = 0; |
|
- | 505 | TimeSinceRC_AboveThreshhold = 0; |