diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 20f766999c..356155c487 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 1; + static const uint16_t k_format_version = 2; // // Parameter identities. diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index e09a4932ad..371a69f7a1 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -38,36 +38,43 @@ void init_rc_out() motor_armed = 1; #endif - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.Init(); // APM Radio initialization - for(byte i = 0; i < 10; i++){ + // fix for crazy output + OCR1B = 0xFFFF; // PB6, OUT3 + OCR1C = 0xFFFF; // PB7, OUT4 + OCR5B = 0xFFFF; // PL4, OUT1 + OCR5C = 0xFFFF; // PL5, OUT2 + OCR4B = 0xFFFF; // PH4, OUT6 + OCR4C = 0xFFFF; // PH5, OUT5 + + if(g.rc_3.radio_min <= 1200){ + output_min(); + } + + for(byte i = 0; i < 5; i++){ delay(20); read_radio(); } - // sanity check on the EEPROM values for radio_min - if(abs(g.rc_3.radio_min - g.rc_3.radio_in) > 40){ - g.rc_3.radio_min = g.rc_3.radio_in; - } + // sanity check + if(g.rc_3.radio_min >= 1300){ + g.rc_3.radio_min = g.rc_3.radio_in; + output_min(); + } +} +void output_min() +{ APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); } - void read_radio() { g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 12153d75b9..484b89233d 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -49,6 +49,13 @@ setup_mode(uint8_t argc, const Menu::arg *argv) //"and then the 'radio' command to configure for your radio.\n" //"\n")); + if(g.rc_1.radio_min >= 1300){ + delay(1000); + Serial.printf_P(PSTR("\n!!!Warning, your radio is not configured!!!")); + delay(1000); + Serial.printf_P(PSTR("\n Type 'radio' to configure now.\n\n")); + } + // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); } @@ -94,6 +101,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) if (('y' != c) && ('Y' != c)) return(-1); + AP_Var::erase_all(); Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM")); @@ -174,6 +182,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) g.rc_8.update_min_max(); if(Serial.available() > 0){ + delay(20); Serial.flush(); save_EEPROM_radio(); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 60fee7f8ef..ad5e3779a2 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -41,7 +41,6 @@ MENU(main_menu, "ArduPilotMega", main_menu_commands); void init_ardupilot() { - byte last_log_num; int last_log_start; int last_log_end; @@ -174,7 +173,7 @@ void init_ardupilot() "Entering interactive setup mode...\n" "\n" "Type 'help' to list commands, 'exit' to leave a submenu.\n" - "Visit the 'setup' menu for first-time configuration.\n")); + "Visit the 'setup' menu for first-time configuration.\n\n")); for (;;) { //Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); main_menu.run();