|
|
|
@ -1,9 +1,9 @@
@@ -1,9 +1,9 @@
|
|
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
|
/***************************************************************************** |
|
|
|
|
The init_ardupilot function processes everything we need for an in - air restart |
|
|
|
|
We will determine later if we are actually on the ground and process a |
|
|
|
|
ground start in that case. |
|
|
|
|
|
|
|
|
|
* The init_ardupilot function processes everything we need for an in - air restart |
|
|
|
|
* We will determine later if we are actually on the ground and process a |
|
|
|
|
* ground start in that case. |
|
|
|
|
* |
|
|
|
|
*****************************************************************************/ |
|
|
|
|
|
|
|
|
|
#if CLI_ENABLED == ENABLED |
|
|
|
@ -112,7 +112,7 @@ static void init_ardupilot()
@@ -112,7 +112,7 @@ static void init_ardupilot()
|
|
|
|
|
// Initialize the timer scheduler to use the ISR registry. |
|
|
|
|
// |
|
|
|
|
|
|
|
|
|
timer_scheduler.init( & isr_registry ); |
|
|
|
|
timer_scheduler.init( &isr_registry ); |
|
|
|
|
|
|
|
|
|
// initialise the analog port reader |
|
|
|
|
AP_AnalogSource_Arduino::init_timer(&timer_scheduler); |
|
|
|
@ -159,9 +159,9 @@ static void init_ardupilot()
@@ -159,9 +159,9 @@ static void init_ardupilot()
|
|
|
|
|
|
|
|
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE |
|
|
|
|
|
|
|
|
|
#if CONFIG_ADC == ENABLED |
|
|
|
|
#if CONFIG_ADC == ENABLED |
|
|
|
|
adc.Init(&timer_scheduler); // APM ADC library initialization |
|
|
|
|
#endif |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// initialise the analog port reader |
|
|
|
|
AP_AnalogSource_Arduino::init_timer(&timer_scheduler); |
|
|
|
@ -211,8 +211,8 @@ static void init_ardupilot()
@@ -211,8 +211,8 @@ static void init_ardupilot()
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
setup the 'main loop is dead' check. Note that this relies on |
|
|
|
|
the RC library being initialised. |
|
|
|
|
* setup the 'main loop is dead' check. Note that this relies on |
|
|
|
|
* the RC library being initialised. |
|
|
|
|
*/ |
|
|
|
|
timer_scheduler.set_failsafe(failsafe_check); |
|
|
|
|
|
|
|
|
@ -272,10 +272,10 @@ static void startup_ground(void)
@@ -272,10 +272,10 @@ static void startup_ground(void)
|
|
|
|
|
|
|
|
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START")); |
|
|
|
|
|
|
|
|
|
#if(GROUND_START_DELAY > 0) |
|
|
|
|
#if (GROUND_START_DELAY > 0) |
|
|
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay")); |
|
|
|
|
delay(GROUND_START_DELAY * 1000); |
|
|
|
|
#endif |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// Makes the servos wiggle |
|
|
|
|
// step 1 = 1 wiggle |
|
|
|
@ -301,9 +301,9 @@ static void startup_ground(void)
@@ -301,9 +301,9 @@ static void startup_ground(void)
|
|
|
|
|
|
|
|
|
|
// Read in the GPS - see if one is connected |
|
|
|
|
GPS_enabled = false; |
|
|
|
|
for (byte counter = 0; ; counter++) { |
|
|
|
|
for (byte counter = 0;; counter++) { |
|
|
|
|
g_gps->update(); |
|
|
|
|
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){ |
|
|
|
|
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED) { |
|
|
|
|
GPS_enabled = true; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -331,7 +331,7 @@ static void startup_ground(void)
@@ -331,7 +331,7 @@ static void startup_ground(void)
|
|
|
|
|
|
|
|
|
|
static void set_mode(byte mode) |
|
|
|
|
{ |
|
|
|
|
if(control_mode == mode){ |
|
|
|
|
if(control_mode == mode) { |
|
|
|
|
// don't switch modes if we are already in the correct mode. |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -380,11 +380,11 @@ static void check_long_failsafe()
@@ -380,11 +380,11 @@ static void check_long_failsafe()
|
|
|
|
|
{ |
|
|
|
|
// only act on changes |
|
|
|
|
// ------------------- |
|
|
|
|
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){ |
|
|
|
|
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS) { |
|
|
|
|
if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { |
|
|
|
|
failsafe_long_on_event(FAILSAFE_LONG); |
|
|
|
|
} |
|
|
|
|
if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { |
|
|
|
|
if(!rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { |
|
|
|
|
failsafe_long_on_event(FAILSAFE_LONG); |
|
|
|
|
} |
|
|
|
|
if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { |
|
|
|
@ -402,13 +402,13 @@ static void check_short_failsafe()
@@ -402,13 +402,13 @@ static void check_short_failsafe()
|
|
|
|
|
{ |
|
|
|
|
// only act on changes |
|
|
|
|
// ------------------- |
|
|
|
|
if(failsafe == FAILSAFE_NONE){ |
|
|
|
|
if(failsafe == FAILSAFE_NONE) { |
|
|
|
|
if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde |
|
|
|
|
failsafe_short_on_event(FAILSAFE_SHORT); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if(failsafe == FAILSAFE_SHORT){ |
|
|
|
|
if(failsafe == FAILSAFE_SHORT) { |
|
|
|
|
if(!ch3_failsafe) { |
|
|
|
|
failsafe_short_off_event(); |
|
|
|
|
} |
|
|
|
@ -463,14 +463,14 @@ static void update_GPS_light(void)
@@ -463,14 +463,14 @@ static void update_GPS_light(void)
|
|
|
|
|
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data |
|
|
|
|
// --------------------------------------------------------------------- |
|
|
|
|
switch (g_gps->status()) { |
|
|
|
|
case(2): |
|
|
|
|
case (2): |
|
|
|
|
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix. |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case(1): |
|
|
|
|
if (g_gps->valid_read == true){ |
|
|
|
|
case (1): |
|
|
|
|
if (g_gps->valid_read == true) { |
|
|
|
|
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock |
|
|
|
|
if (GPS_light){ |
|
|
|
|
if (GPS_light) { |
|
|
|
|
digitalWrite(C_LED_PIN, LED_OFF); |
|
|
|
|
} else { |
|
|
|
|
digitalWrite(C_LED_PIN, LED_ON); |
|
|
|
@ -499,7 +499,7 @@ static void resetPerfData(void) {
@@ -499,7 +499,7 @@ static void resetPerfData(void) {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
map from a 8 bit EEPROM baud rate to a real baud rate |
|
|
|
|
* map from a 8 bit EEPROM baud rate to a real baud rate |
|
|
|
|
*/ |
|
|
|
|
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) |
|
|
|
|
{ |
|
|
|
@ -539,13 +539,13 @@ static void check_usb_mux(void)
@@ -539,13 +539,13 @@ static void check_usb_mux(void)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
called by gyro/accel init to flash LEDs so user |
|
|
|
|
has some mesmerising lights to watch while waiting |
|
|
|
|
* called by gyro/accel init to flash LEDs so user |
|
|
|
|
* has some mesmerising lights to watch while waiting |
|
|
|
|
*/ |
|
|
|
|
void flash_leds(bool on) |
|
|
|
|
{ |
|
|
|
|
digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON); |
|
|
|
|
digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF); |
|
|
|
|
digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON); |
|
|
|
|
digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|