diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c0054897ea..b7d7007cb8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -359,28 +359,29 @@ static AP_RangeFinder_MaxsonarXL *sonar; //Documentation of GLobals: static union { struct { - uint8_t home_is_set : 1; // 0 - uint8_t simple_mode : 1; // 1 // This is the state of simple mode - uint8_t manual_attitude : 1; // 2 - uint8_t manual_throttle : 1; // 3 + uint8_t home_is_set : 1; // 0 + uint8_t simple_mode : 1; // 1 // This is the state of simple mode + uint8_t manual_attitude : 1; // 2 + uint8_t manual_throttle : 1; // 3 - uint8_t low_battery : 1; // 4 // Used to track if the battery is low - LED output flashes when the batt is low - uint8_t pre_arm_check : 1; // 5 // true if the radio and accel calibration have been performed - uint8_t logging_started : 1; // 6 // true if dataflash logging has started - uint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raised + uint8_t pre_arm_rc_check : 1; // 5 // true if rc input pre-arm checks have been completed successfully + uint8_t pre_arm_check : 1; // 6 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed + uint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raised + uint8_t logging_started : 1; // 8 // true if dataflash logging has started - uint8_t failsafe_radio : 1; // 8 // A status flag for the radio failsafe - uint8_t failsafe_batt : 1; // 9 // A status flag for the battery failsafe - uint8_t failsafe_gps : 1; // 10 // A status flag for the gps failsafe - uint8_t failsafe_gcs : 1; // 11 // A status flag for the ground station failsafe - uint8_t rc_override_active : 1; // 12 // true if rc control are overwritten by ground station - uint8_t do_flip : 1; // 13 // Used to enable flip code - uint8_t takeoff_complete : 1; // 14 - uint8_t land_complete : 1; // 15 - uint8_t compass_status : 1; // 16 - uint8_t gps_status : 1; // 17 + uint8_t low_battery : 1; // 9 // Used to track if the battery is low - LED output flashes when the batt is low + uint8_t failsafe_radio : 1; // 10 // A status flag for the radio failsafe + uint8_t failsafe_batt : 1; // 11 // A status flag for the battery failsafe + uint8_t failsafe_gps : 1; // 12 // A status flag for the gps failsafe + uint8_t failsafe_gcs : 1; // 13 // A status flag for the ground station failsafe + uint8_t rc_override_active : 1; // 14 // true if rc control are overwritten by ground station + uint8_t do_flip : 1; // 15 // Used to enable flip code + uint8_t takeoff_complete : 1; // 16 + uint8_t land_complete : 1; // 17 + uint8_t compass_status : 1; // 18 + uint8_t gps_status : 1; // 19 }; - uint16_t value; + uint32_t value; } ap; @@ -1267,7 +1268,7 @@ static void slow_loop() } #define AUTO_DISARMING_DELAY 25 -// 1Hz loop +// super_slow_loop - runs at 1Hz static void super_slow_loop() { if (g.log_bitmask != 0) { diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index f862546df4..5112178ffe 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -11,7 +11,7 @@ static void arm_motors() { static int16_t arming_counter; - // don't allow arming/disarming in anything but manual + // ensure throttle is down if (g.rc_3.control_in > 0) { arming_counter = 0; return; @@ -147,6 +147,9 @@ static void init_arm_motors() // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); + // enable output to motors + output_min(); + // finally actually arm the motors motors.armed(true); @@ -157,7 +160,7 @@ static void init_arm_motors() failsafe_enable(); } -// perform pre-arm checks and set +// perform pre-arm checks and set ap.pre_arm_check flag static void pre_arm_checks() { // exit immediately if we've already successfully performed the pre-arm check @@ -165,8 +168,9 @@ static void pre_arm_checks() return; } - // check if radio has been calibrated - if(!g.rc_3.radio_min.load()) { + // pre-arm rc checks a prerequisite + pre_arm_rc_checks(); + if(!ap.pre_arm_rc_check) { return; } @@ -191,6 +195,28 @@ static void pre_arm_checks() ap.pre_arm_check = true; } +// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag +static void pre_arm_rc_checks() +{ + // exit immediately if we've already successfully performed the pre-arm rc check + if( ap.pre_arm_rc_check ) { + return; + } + + // check if radio has been calibrated + if(!g.rc_3.radio_min.load()) { + return; + } + + // check if throttle min is reasonable + if(g.rc_3.radio_min > 1300) { + return; + } + + // if we've gotten this far rc is ok + ap.pre_arm_rc_check = true; +} + static void init_disarm_motors() { #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 3db0611f2e..78cf3c94fc 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -61,6 +61,7 @@ static void init_rc_in() #endif } + // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration static void init_rc_out() { motors.set_update_rate(g.rc_speed); @@ -77,18 +78,11 @@ static void init_rc_out() // we want the input to be scaled correctly g.rc_3.set_range_out(0,1000); - // sanity check - prevent unconfigured radios from outputting - if(g.rc_3.radio_min >= 1300) { - g.rc_3.radio_min = g.rc_3.radio_in; - } - - // we are full throttle + // full throttle means to enter ESC calibration if(g.rc_3.control_in >= (MAXIMUM_THROTTLE - 50)) { if(g.esc_calibrate == 0) { // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(1); - // send minimum throttle out to ESC - motors.output_min(); // display message on console cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n")); // block until we restart @@ -107,8 +101,11 @@ static void init_rc_out() // did we abort the calibration? if(g.esc_calibrate == 1) g.esc_calibrate.set_and_save(0); + } - // send miinimum throttle out to ESC + // enable output to motors + pre_arm_rc_checks(); + if (ap.pre_arm_rc_check) { output_min(); } @@ -119,6 +116,7 @@ static void init_rc_out() #endif } +// output_min - enable and output lowest possible value to motors void output_min() { // enable motors diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index d26454d30a..95143b7788 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -472,6 +472,13 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) comp_type = AP_COMPASS_MOT_COMP_CURRENT; } + // check if radio is calibration + pre_arm_rc_checks(); + if(!ap.pre_arm_rc_check) { + cliSerial->print_P(PSTR("radio not calibrated, exiting")); + return 0; + } + // check compass is enabled if( !g.compass_enabled ) { cliSerial->print_P(PSTR("compass disabled, exiting")); @@ -549,8 +556,8 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) // enable motors and pass through throttle init_rc_out(); + output_min(); motors.armed(true); - motors.output_min(); // initialise run time last_run_time = millis(); @@ -1370,6 +1377,8 @@ init_esc() { // reduce update rate to motors to 50Hz motors.set_update_rate(50); + + // we enable the motors directly here instead of calling output_min because output_min would send a low signal to the ESC and disrupt the calibration process motors.enable(); motors.armed(true); while(1) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index cb6f367f6e..d9e2280f94 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -184,7 +184,8 @@ static void init_ardupilot() #endif init_rc_in(); // sets up rc channels from radio - init_rc_out(); // sets up the timer libs + init_rc_out(); // sets up motors and output to escs + /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 75ac025c35..60f47ce70c 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -320,6 +320,13 @@ test_tuning(uint8_t argc, const Menu::arg *argv) static int8_t test_battery(uint8_t argc, const Menu::arg *argv) { + // check if radio is calibration + pre_arm_rc_checks(); + if(!ap.pre_arm_rc_check) { + cliSerial->print_P(PSTR("radio not calibrated, exiting")); + return(0); + } + cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n")); while (cliSerial->read() != -1); /* flush */ while(!cliSerial->available()) { /* wait for input */ @@ -329,7 +336,7 @@ test_battery(uint8_t argc, const Menu::arg *argv) print_hit_enter(); // allow motors to spin - motors.enable(); + output_min(); motors.armed(true); while(1) {