diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index bf5088d90c..8f62f21890 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -235,6 +235,7 @@ boolean rate_yaw_flag; // used to transition yaw control from Rate control // ---------- boolean motor_light; // status of the Motor safety boolean GPS_light; // status of the GPS light +boolean timer_light; // status of the Motor safety // GPS variables // ------------- @@ -427,7 +428,7 @@ unsigned long elapsedTime; // for doing custom events float load; // % MCU cycles used byte counter_one_herz; -bool GPS_disabled = true; +bool GPS_enabled = false; //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -448,9 +449,14 @@ void loop() G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator mainLoop_count++; - //if(delta_ms_fast_loop > 11){ - // Serial.println(delta_ms_fast_loop,DEC); - //} + /* + if(delta_ms_fast_loop > 11){ + update_timer_light(true); + //Serial.println(delta_ms_fast_loop,DEC); + }else{ + update_timer_light(false); + }*/ + // Execute the fast loop // --------------------- fast_loop(); @@ -525,7 +531,7 @@ void medium_loop() medium_loopCounter++; - if(GPS_disabled == false){ + if(GPS_enabled){ update_GPS(); } @@ -972,7 +978,7 @@ void update_current_flight_mode(void) if(flight_timer > 10){ flight_timer = 0; - if(GPS_disabled){ + if(GPS_enabled == false){ current_loc.lat = home.lat = 0; current_loc.lng = home.lng = 0; } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 484b89233d..23e40aaa8a 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -69,6 +69,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) // clear the area print_blanks(8); + report_version(); report_radio(); report_frame(); report_current(); @@ -807,6 +808,14 @@ void report_current() print_blanks(2); } +void report_gps() +{ + Serial.printf_P(PSTR("\nGPS\n")); + print_divider(); + print_enabled(GPS_enabled); + print_blanks(2); +} + void report_sonar() { g.sonar_enabled.load(); @@ -816,6 +825,13 @@ void report_sonar() print_blanks(2); } +void report_version() +{ + Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get()); + print_divider(); + print_blanks(2); +} + void report_frame() { diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 6e87090688..04fa68b15d 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -85,11 +85,15 @@ void init_ardupilot() "\n\nFree RAM: %lu\n"), freeRAM()); + // // Check the EEPROM format version before loading any parameters from EEPROM. // + report_version(); + if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { + Serial.printf_P(PSTR("\n\nForcing complete parameter reset...")); /*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" "\n\nForcing complete parameter reset..."), @@ -255,20 +259,22 @@ void startup_ground(void) init_commands(); byte counter = 4; - GPS_disabled = true; + GPS_enabled = false; // Read in the GPS for (byte counter = 0; ; counter++) { g_gps->update(); if (g_gps->status() != 0){ + GPS_enabled = true; break; } if (counter >= 4) { - GPS_disabled = true; + GPS_enabled = false; break; } } - + report_gps(); + SendDebug("\nReady to FLY "); gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } @@ -410,6 +416,15 @@ void update_motor_light(void) } } +void update_timer_light(bool light) +{ + if(light == true){ + digitalWrite(B_LED_PIN, HIGH); + }else{ + digitalWrite(B_LED_PIN, LOW); + } +} + void resetPerfData(void) { /*