diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 69cde548e0..9071983d05 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -844,6 +844,13 @@ AP_Relay relay; // is to take the 0 to 1024 range down to an 8 bit range for MAVLink AP_HAL::AnalogSource* rssi_analog_source; + +// Input sources for battery voltage, battery current, board vcc +AP_HAL::AnalogSource* batt_volt_analog_source; +AP_HAL::AnalogSource* batt_curr_analog_source; +AP_HAL::AnalogSource* board_vcc_analog_source; + + #if CLI_ENABLED == ENABLED static int8_t setup_show (uint8_t argc, const Menu::arg *argv); #endif @@ -901,7 +908,10 @@ void setup() { &sonar_mode_filter); #endif - rssi_analog_source = hal.analogin->channel(g.rssi_pin, 0.25); + rssi_analog_source = hal.analogin->channel(g.rssi_pin, 0.25); + batt_volt_analog_source = hal.analogin->channel(g.battery_volt_pin); + batt_curr_analog_source = hal.analogin->channel(g.battery_curr_pin); + board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); memcheck_init(); init_ardupilot(); diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index c42a3a4700..b24734eb20 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -82,14 +82,12 @@ static void read_battery(void) } if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino batt_volt_pin(g.battery_volt_pin); - batt_volt_pin.set_pin(g.battery_volt_pin); - battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin.read_average()); + batt_volt_analog_source->set_pin(g.battery_volt_pin); + battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source->read_average()); } if(g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino batt_curr_pin(g.battery_curr_pin); - batt_curr_pin.set_pin(g.battery_curr_pin); - current_amps1 = CURRENT_AMPS(batt_curr_pin.read_average()); + batt_curr_analog_source->set_pin(g.battery_curr_pin); + current_amps1 = CURRENT_AMPS(batt_curr_analog_source->read_average()); current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) } @@ -111,6 +109,6 @@ static void read_battery(void) void read_receiver_rssi(void) { rssi_analog_source->set_pin(g.rssi_pin); - float ret = rssi_analog_source->read(); + float ret = rssi_analog_source->read_latest(); receiver_rssi = constrain(ret, 0, 255); } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index d3044cb366..bae45d7cef 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -207,7 +207,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) if(cliSerial->available() > 0) { delay(20); - cliSerial->flush(); + while (cliSerial->read() != -1); /* flush */ g.rc_1.save_eeprom(); g.rc_2.save_eeprom(); @@ -249,8 +249,8 @@ setup_accel(uint8_t argc, const Menu::arg *argv) { ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); - ins.init_accel(delay, flash_leds); + flash_leds); + ins.init_accel(flash_leds); report_ins(); return(0); } @@ -263,7 +263,7 @@ static void setup_printf_P(const prog_char_t *fmt, ...) { va_list arg_list; va_start(arg_list, fmt); - cliSerial->vprintf_P(fmt, arg_list); + cliSerial->printf_P(fmt, arg_list); va_end(arg_list); } @@ -286,8 +286,8 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv) cliSerial->println_P(PSTR("Initialising gyros")); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); - ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key); + flash_leds); + ins.calibrate_accel(flash_leds, cliSerial); report_ins(); return(0); } @@ -986,12 +986,10 @@ print_done() static void zero_eeprom(void) { - uint8_t b = 0; - cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); - for (uintptr_t i = 0; i < EEPROM_MAX_ADDR; i++) { - eeprom_write_byte((uint8_t *) i, b); + for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) { + hal.storage->write_byte(i, 0); } cliSerial->printf_P(PSTR("done\n")); @@ -1073,7 +1071,7 @@ static bool wait_for_yes() { int c; - cliSerial->flush(); + while (cliSerial->read() != -1); /* flush */ cliSerial->printf_P(PSTR("Y to save\n")); do { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a3ff66d98c..3847e05f64 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -86,42 +86,20 @@ static void init_ardupilot() // The console port buffers are defined to be sufficiently large to support // the MAVLink protocol efficiently // - cliSerial->begin(SERIAL0_BAUD, 128, 256); + hal.uartA->begin(SERIAL0_BAUD, 128, 256); // GPS serial port. // #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // standard gps running. Note that we need a 256 byte buffer for some // GPS types (eg. UBLOX) - Serial1.begin(38400, 256, 16); + hal.uartB->begin(38400, 256, 16); #endif cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n"), memcheck_available_memory()); - // - // Initialize Wire and SPI libraries - // -#ifndef DESKTOP_BUILD - I2c.begin(); - I2c.timeOut(5); - // initially set a fast I2c speed, and drop it on first failures - I2c.setSpeed(true); -#endif - SPI.begin(); - SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate - -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - SPI3.begin(); - SPI3.setSpeed(SPI3_SPEED_2MHZ); -#endif - - // - // Initialize the isr_registry. - // - isr_registry.init(); - // // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) // @@ -143,8 +121,9 @@ static void init_ardupilot() #if CONFIG_PUSHBUTTON == ENABLED pinMode(PUSHBUTTON_PIN, INPUT); // unused #endif + #if CONFIG_RELAY == ENABLED - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + relay.init(); #endif #if COPTER_LEDS == ENABLED @@ -168,7 +147,7 @@ static void init_ardupilot() load_parameters(); // init the GCS - gcs0.init(&Serial); + gcs0.init(hal.uartA); #if USB_MUX_PIN > 0 if (!ap_system.usb_connected) { @@ -178,8 +157,8 @@ static void init_ardupilot() } #else // we have a 2nd serial port for telemetry - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256); - gcs3.init(&Serial3); + hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256); + gcs3.init(hal.uartC); #endif // identify ourselves correctly with the ground station @@ -200,42 +179,26 @@ static void init_ardupilot() } #endif -/* -#ifdef RADIO_OVERRIDE_DEFAULTS - { - int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; - APM_RC.setHIL(rc_override); - } -#endif -*/ - #if FRAME_CONFIG == HELI_FRAME motors.servo_manual = false; motors.init_swash(); // heli initialisation #endif - RC_Channel::set_apm_rc(&APM_RC); init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs - - timer_scheduler.init( &isr_registry ); - /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ - timer_scheduler.set_failsafe(failsafe_check); - - // initialise the analog port reader - AP_AnalogSource_Arduino::init_timer(&timer_scheduler); + hal.scheduler->register_timer_failsafe(failsafe_check, 1000); #if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_ADC == ENABLED // begin filtering the ADC Gyros - adc.Init(&timer_scheduler); // APM ADC library initialization + adc.Init(); // APM ADC library initialization #endif // CONFIG_ADC - barometer.init(&timer_scheduler); + barometer.init(); #endif // HIL_MODE @@ -277,7 +240,7 @@ static void init_ardupilot() const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n"); cliSerial->println_P(msg); #if USB_MUX_PIN == 0 - Serial3.println_P(msg); + hal.uartC->println_P(msg); #endif #endif // CLI_ENABLED @@ -308,25 +271,29 @@ init_rate_controllers(); startup_ground(); - // now that initialisation of IMU has occurred increase SPI to 2MHz - SPI.setClockDivider(SPI_CLOCK_DIV8); // 2MHZ SPI rate - #if LOGGING_ENABLED == ENABLED Log_Write_Startup(); #endif + init_ap_limits(); + + cliSerial->print_P(PSTR("\nReady to FLY ")); +} + /////////////////////////////////////////////////////////////////////////////// -// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters +// Experimental AP_Limits library - set constraints, limits, fences, minima, +// maxima on various parameters //////////////////////////////////////////////////////////////////////////////// +static void init_ap_limits() { #ifdef AP_LIMITS + // AP_Limits modules are stored as a _linked list_. That allows us to + // define an infinite number of modules and also to allocate no space until + // we actually need to. - // AP_Limits modules are stored as a _linked list_. That allows us to define an infinite number of modules - // and also to allocate no space until we actually need to. - - // The linked list looks (logically) like this - // [limits module] -> [first limit module] -> [second limit module] -> [third limit module] -> NULL + // The linked list looks (logically) like this [limits module] -> [first + // limit module] -> [second limit module] -> [third limit module] -> NULL // The details of the linked list are handled by the methods @@ -347,16 +314,13 @@ init_rate_controllers(); m = limits.modules_next(); } } - #endif - - cliSerial->print_P(PSTR("\nReady to FLY ")); } -//******************************************************************************** +//****************************************************************************** //This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** +//****************************************************************************** static void startup_ground(void) { gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START")); @@ -365,13 +329,13 @@ static void startup_ground(void) // ----------------------------- ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - mavlink_delay, flash_leds, &timer_scheduler); + flash_leds); #if CLI_ENABLED == ENABLED report_ins(); #endif // initialise ahrs (may push imu calibration into the mpu6000 if using that device). - ahrs.init(&timer_scheduler); + ahrs.init(); // setup fast AHRS gains to get right attitude ahrs.set_fast_gains(true); @@ -662,8 +626,7 @@ void flash_leds(bool on) */ uint16_t board_voltage(void) { - static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC); - return vcc.read_vcc(); + return board_vcc_analog_source->read_latest(); } #endif diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index c94fc5ac35..7ef09a0814 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -37,7 +37,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); //static int8_t test_mission(uint8_t argc, const Menu::arg *argv); // this is declared here to remove compiler errors -extern void print_latlon(BetterStream *s, int32_t lat_or_lon); // in Log.pde +extern void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon); // in Log.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -104,13 +104,14 @@ test_mode(uint8_t argc, const Menu::arg *argv) static int8_t test_eedump(uint8_t argc, const Menu::arg *argv) { - uintptr_t i, j; // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { + for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i += 16) { cliSerial->printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); + for (uint16_t j = 0; j < 16; j++) { + int b = hal.storage->read_byte(i+j); + cliSerial->printf_P(PSTR(" %02x"), b); + } cliSerial->println(); } return(0); @@ -462,7 +463,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); + flash_leds); delay(50); @@ -509,9 +510,9 @@ test_gps(uint8_t argc, const Menu::arg *argv) if (g_gps->new_data) { cliSerial->printf_P(PSTR("Lat: ")); - print_latlon(&Serial, g_gps->latitude); + print_latlon(cliSerial, g_gps->latitude); cliSerial->printf_P(PSTR(", Lon ")); - print_latlon(&Serial, g_gps->longitude); + print_latlon(cliSerial, g_gps->longitude); cliSerial->printf_P(PSTR(", Alt: %ldm, #sats: %d\n"), g_gps->altitude/100, g_gps->num_sats); @@ -665,11 +666,11 @@ test_battery(uint8_t argc, const Menu::arg *argv) return (0); #else cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n")); - cliSerial->flush(); - while(!cliSerial->available()) { + while (cliSerial->read() != -1); /* flush */ + while(!cliSerial->available()) { /* wait for input */ delay(100); } - cliSerial->flush(); + while (cliSerial->read() != -1); /* flush */ print_hit_enter(); // allow motors to spin @@ -921,7 +922,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) while(1) { delay(100); - cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar.read()); + cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar->read()); //cliSerial->printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value); if(cliSerial->available() > 0) { diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index bb5feed4db..e58b47c36b 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -1,7 +1,7 @@ //////////////////////////////////////////////////////////////////////////////// // Toy Mode - THOR //////////////////////////////////////////////////////////////////////////////// -static boolean CH7_toy_flag; +static bool CH7_toy_flag; #if TOY_MIXER == TOY_LOOKUP_TABLE static const int16_t toy_lookup[] = {