diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1866c5dfab..2a2829e2d4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -54,64 +54,44 @@ // Header includes //////////////////////////////////////////////////////////////////////////////// -// AVR runtime -#include -#include -#include -#include -#include - // Libraries -#include + +// Common dependencies #include #include #include #include -#include -#include // ArduPilot Mega RC Library +// AP_HAL +#include +#include +#include +// Application dependencies +#include // MAVLink GCS definitions #include // ArduPilot GPS library -#include // Arduino I2C lib -#include // Arduino SPI lib -#include // SPI3 library -#include // for removing conflict between optical flow and dataflash on SPI3 bus #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library -#include #include #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library #include // Curve used to linearlise throttle pwm to thrust #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library -#include // Parent header of Timer - // (only included for makefile libpath to work) -#include // TimerProcess is the scheduler for MPU6000 reads. #include #include // PI library #include // PID library #include // RC Channel Library #include // AP Motors library -#include // AP Motors library for Quad -#include // AP Motors library for Tri -#include // AP Motors library for Hexa -#include // AP Motors library for Y6 -#include // AP Motors library for Octa -#include // AP Motors library for OctaQuad -#include // AP Motors library for Heli -#include // AP Motors library for Heli #include // Range finder library #include // Optical Flow library #include // Filter library #include // APM FIFO Buffer -#include // Mode Filter from Filter library -#include // Mode Filter from Filter library #include // GPS Lead filter -#include // Low Pass Filter library #include // APM relay #include // Photo or video camera #include // Camera/Antenna mount #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library -#include // faster digital write for LEDs +#include // ArduPilot Mega Declination Helper Library +#include #include // Configuration @@ -119,43 +99,24 @@ #include "config.h" #include "config_channels.h" -#include // MAVLink GCS definitions - // Local modules #include "Parameters.h" #include "GCS.h" -#include // ArduPilot Mega Declination Helper Library - -// Limits library - Puts limits on the vehicle, and takes recovery actions -#include -#include // a limits library module -#include // a limits library module -#include // a limits library module - - //////////////////////////////////////////////////////////////////////////////// -// Serial ports +// AP_HAL instance //////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -FastSerialPort3(Serial3); // Telemetry port - -// port to use for command line interface -static FastSerial *cliSerial = &Serial; - -// this sets up the parameter table, and sets the default values. This -// must be the first AP_Param variable declared to ensure its -// constructor runs before the constructors of the other AP_Param -// variables -AP_Param param_loader(var_info, WP_START_BYTE); +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 +const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL +const AP_HAL::HAL& hal = AP_HAL_AVR_SITL; +#include +SITL sitl; +#endif -Arduino_Mega_ISR_Registry isr_registry; +AP_HAL::BetterStream* cliSerial; //////////////////////////////////////////////////////////////////////////////// // Parameters @@ -168,26 +129,16 @@ static Parameters g; //////////////////////////////////////////////////////////////////////////////// // prototypes -static void update_events(void); - -//////////////////////////////////////////////////////////////////////////////// -// RC Hardware //////////////////////////////////////////////////////////////////////////////// -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -APM_RC_APM2 APM_RC; -#else -APM_RC_APM1 APM_RC; -#endif +static void update_events(void); //////////////////////////////////////////////////////////////////////////////// // Dataflash //////////////////////////////////////////////////////////////////////////////// -AP_Semaphore spi_semaphore; -AP_Semaphore spi3_semaphore; -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -DataFlash_APM2 DataFlash(&spi3_semaphore); +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 +DataFlash_APM2 DataFlash; #else -DataFlash_APM1 DataFlash(&spi_semaphore); +DataFlash_APM1 DataFlash; #endif @@ -213,7 +164,7 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor: static GPS *g_gps; // flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; +static AP_Int8 *flight_modes = &g.flight_mode1; #if HIL_MODE == HIL_MODE_DISABLED @@ -222,15 +173,13 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_ADC_ADS7844 adc; #endif - #ifdef DESKTOP_BUILD + #if CONFIG_HIL_BOARD == HIL_BOARD_AVR_SITL AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; - #include -SITL sitl; #else #if CONFIG_BARO == AP_BARO_BMP085 - # if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + # if CONFIG_HAL_BOARD == HAL_BOARD_APM2 AP_Baro_BMP085 barometer(true); # else AP_Baro_BMP085 barometer(false); @@ -243,7 +192,7 @@ AP_Compass_HMC5843 compass; #endif #if OPTFLOW == ENABLED - #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); #else AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); @@ -254,22 +203,22 @@ AP_OpticalFlow optflow; // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO -AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); +AP_GPS_Auto g_gps_driver(hal.uartB, &g_gps); #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA -AP_GPS_NMEA g_gps_driver(&Serial1); +AP_GPS_NMEA g_gps_driver(hal.uartB); #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF -AP_GPS_SIRF g_gps_driver(&Serial1); +AP_GPS_SIRF g_gps_driver(hal.uartB); #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX -AP_GPS_UBLOX g_gps_driver(&Serial1); +AP_GPS_UBLOX g_gps_driver(hal.uartB); #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK -AP_GPS_MTK g_gps_driver(&Serial1); +AP_GPS_MTK g_gps_driver(hal.uartB); #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 -AP_GPS_MTK16 g_gps_driver(&Serial1); +AP_GPS_MTK16 g_gps_driver(hal.uartB); #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE AP_GPS_None g_gps_driver(NULL); @@ -288,14 +237,14 @@ AP_InertialSensor_Oilpan ins(&adc); // a NULL GPS object pointer static GPS *g_gps_null; - #if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #if DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2 AP_AHRS_MPU6000 ahrs(&ins, g_gps); // only works with APM2 #else AP_AHRS_DCM ahrs(&ins, g_gps); #endif // ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM - #if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2 AP_AHRS_MPU6000 ahrs2(&ins, g_gps); // only works with APM2 #endif @@ -320,25 +269,18 @@ AP_Compass_HIL compass; // never used AP_Baro_BMP085_HIL barometer; #if OPTFLOW == ENABLED - #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); #else AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); - #endif // CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #endif // CONFIG_HAL_BOARD == HAL_BOARD_APM2 #endif // OPTFLOW == ENABLED - #ifdef DESKTOP_BUILD - #include -SITL sitl; - #endif // DESKTOP_BUILD static int32_t gps_base_alt; #else #error Unrecognised HIL_MODE setting. #endif // HIL MODE -// we always have a timer scheduler -AP_TimerProcess timer_scheduler; - //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// @@ -443,7 +385,7 @@ static int16_t y_rate_error; static int8_t control_mode = STABILIZE; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch -static byte oldSwitchPosition; +static uint8_t oldSwitchPosition; // receiver RSSI static uint8_t receiver_rssi; @@ -475,11 +417,11 @@ static uint8_t receiver_rssi; #endif #if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); +MOTOR_CLASS motors(CONFIG_HAL_BOARD, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); #elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); +MOTOR_CLASS motors(CONFIG_HAL_BOARD, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); #else -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); +MOTOR_CLASS motors(CONFIG_HAL_BOARD, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); #endif //////////////////////////////////////////////////////////////////////////////// @@ -507,11 +449,11 @@ float tuning_value; //////////////////////////////////////////////////////////////////////////////// // This is current status for the LED lights state machine // setting this value changes the output of the LEDs -static byte led_mode = NORMAL_LEDS; +static uint8_t led_mode = NORMAL_LEDS; // Blinking indicates GPS status -static byte copter_leds_GPS_blink; +static uint8_t copter_leds_GPS_blink; // Blinking indicates battery status -static byte copter_leds_motor_blink; +static uint8_t copter_leds_motor_blink; // Navigation confirmation blinks static int8_t copter_leds_nav_blink; @@ -552,7 +494,7 @@ union float_int { static int32_t wp_bearing; // Status of the Waypoint tracking mode. Options include: // NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE -static byte wp_control; +static uint8_t wp_control; // Register containing the index of the current navigation command in the mission script static int16_t command_nav_index; // Register containing the index of the previous navigation command in the mission script @@ -824,7 +766,7 @@ static int16_t yaw_look_at_heading_slew; // Repeat Mission Scripting Command //////////////////////////////////////////////////////////////////////////////// // The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc -static byte event_id; +static uint8_t event_id; // Used to manage the timimng of repeating events static uint32_t event_timer; // How long to delay the next firing of event in millis @@ -872,11 +814,11 @@ static uint32_t fast_loopTimer; // Time in microseconds of 50hz control loop static uint32_t fiftyhz_loopTimer; // Counters for branching from 10 hz control loop -static byte medium_loopCounter; +static uint8_t medium_loopCounter; // Counters for branching from 3 1/3hz control loop -static byte slow_loopCounter; +static uint8_t slow_loopCounter; // Counters for branching at 1 hz -static byte counter_one_herz; +static uint8_t counter_one_herz; // Counter of main loop executions. Used for performance monitoring and failsafe processing static uint16_t mainLoop_count; // Delta Time in milliseconds for navigation computations, updated with every good GPS read @@ -1017,7 +959,7 @@ void loop() } } } else { -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL usleep(1000); #endif if (timer - fast_loopTimer < 9) { @@ -1412,7 +1354,7 @@ static void update_optical_flow(void) static void update_GPS(void) { // A counter that is used to grab at least 10 reads before commiting the Home location - static byte ground_start_count = 10; + static uint8_t ground_start_count = 10; g_gps->update(); update_GPS_light(); @@ -1739,7 +1681,7 @@ void update_roll_pitch_mode(void) // new radio frame is used to make sure we only call this at 50hz void update_simple_mode(void) { - static byte simple_counter = 0; // State machine counter for Simple Mode + static uint8_t simple_counter = 0; // State machine counter for Simple Mode static float simple_sin_y=0, simple_cos_x=0; // used to manage state machine diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 189171866a..03e840f20e 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -7,11 +7,8 @@ #ifndef __GCS_H #define __GCS_H -#include -#include +#include #include -#include -#include /// /// @class GCS @@ -40,7 +37,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(FastSerial *port) { + void init(AP_HAL::UARTDriver *port) { _port = port; initialised = true; last_gps_satellites = 255; @@ -91,7 +88,7 @@ public: protected: /// The stream we are communicating over - FastSerial * _port; + AP_HAL::UARTDriver * _port; }; // @@ -110,7 +107,7 @@ class GCS_MAVLINK : public GCS_Class public: GCS_MAVLINK(); void update(void); - void init(FastSerial *port); + void init(AP_HAL::UARTDriver *port); void send_message(enum ap_message id); void send_text(gcs_severity severity, const char *str); void send_text(gcs_severity severity, const prog_char_t *str); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index a1487777bd..a6d38f5fbc 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -761,10 +761,10 @@ GCS_MAVLINK::GCS_MAVLINK() : } void -GCS_MAVLINK::init(FastSerial * port) +GCS_MAVLINK::init(AP_HAL::UARTDriver* port) { GCS_Class::init(port); - if (port == &Serial) { + if (port == hal.uartA) { mavlink_comm_0_port = port; chan = MAVLINK_COMM_0; }else{ diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 49c4cf0c0f..58b5b1cfba 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -228,7 +228,7 @@ process_logs(uint8_t argc, const Menu::arg *argv) // print_latlon - prints an latitude or longitude value held in an int32_t // probably this should be moved to AP_Common -void print_latlon(BetterStream *s, int32_t lat_or_lon) +void print_latlon(AL_HAL::BetterStream *s, int32_t lat_or_lon) { int32_t dec_portion, frac_portion; int32_t abs_lat_or_lon = labs(lat_or_lon); @@ -703,7 +703,7 @@ static void Log_Read_Performance() } // Write a command processing packet. Total length : 21 bytes -static void Log_Write_Cmd(byte num, struct Location *wp) +static void Log_Write_Cmd(uint8_t num, struct Location *wp) { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -851,7 +851,7 @@ static void Log_Read_INAV() } // Write a mode packet. Total length : 7 bytes -static void Log_Write_Mode(byte mode) +static void Log_Write_Mode(uint8_t mode) { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -1131,8 +1131,8 @@ static void Log_Read(int16_t start_page, int16_t end_page) // Read the DataFlash log memory : Packet Parser static int16_t Log_Read_Process(int16_t start_page, int16_t end_page) { - byte data; - byte log_step = 0; + uint8_t data; + uint8_t log_step = 0; int16_t page = start_page; int16_t packet_count = 0; @@ -1257,9 +1257,9 @@ static void Log_Read_Startup() { } static void Log_Read(int16_t start_page, int16_t end_page) { } -static void Log_Write_Cmd(byte num, struct Location *wp) { +static void Log_Write_Cmd(uint8_t num, struct Location *wp) { } -static void Log_Write_Mode(byte mode) { +static void Log_Write_Mode(uint8_t mode) { } static void Log_Write_Raw() { } diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 086ab902b0..62a3a2c56a 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -5,7 +5,7 @@ static void read_control_switch() { static uint8_t switch_counter = 0; - byte switchPosition = readSwitch(); + uint8_t switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition) { switch_counter++; diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 28949b17c0..59dec83af8 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -67,7 +67,7 @@ static void update_motor_light(void) static void dancing_light() { - static byte step; + static uint8_t step; if (step++ == 3) step = 0; @@ -368,4 +368,4 @@ void piezo_beep(){ piezo_off(); } -#endif //COPTER_LEDS \ No newline at end of file +#endif //COPTER_LEDS diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 0f2bad5360..53de483c35 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -68,7 +68,7 @@ static void init_rc_out() motors.set_min_throttle(g.throttle_min); motors.set_max_throttle(g.throttle_max); - for(byte i = 0; i < 5; i++) { + for(uint8_t i = 0; i < 5; i++) { delay(20); read_radio(); } @@ -199,7 +199,7 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm) static void trim_radio() { - for (byte i = 0; i < 30; i++) { + for (uint8_t i = 0; i < 30; i++) { read_radio(); } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 33d4ee551d..d3044cb366 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -316,8 +316,8 @@ setup_frame(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte _switchPosition = 0; - byte _oldSwitchPosition = 0; + uint8_t _switchPosition = 0; + uint8_t _oldSwitchPosition = 0; int8_t mode = 0; cliSerial->printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n")); @@ -780,10 +780,10 @@ static void report_batt_monitor() print_blanks(2); } -static void report_wp(byte index = 255) +static void report_wp(uint8_t index = 255) { if(index == 255) { - for(byte i = 0; i < g.command_total; i++) { + for(uint8_t i = 0; i < g.command_total; i++) { struct Location temp = get_cmd_with_index(i); print_wp(&temp, i); } @@ -966,7 +966,7 @@ print_radio_values() } static void -print_switch(byte p, byte m, bool b) +print_switch(uint8_t p, uint8_t m, bool b) { cliSerial->printf_P(PSTR("Pos %d:\t"),p); print_flight_mode(m); @@ -986,7 +986,7 @@ print_done() static void zero_eeprom(void) { - byte b = 0; + uint8_t b = 0; cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); @@ -1038,8 +1038,8 @@ heli_get_servo(int16_t servo_num){ // Used to read integer values from the serial port static int16_t read_num_from_serial() { - byte index = 0; - byte timeout = 0; + uint8_t index = 0; + uint8_t timeout = 0; char data[5] = ""; do { @@ -1118,7 +1118,7 @@ init_esc() } } -static void print_wp(struct Location *cmd, byte index) +static void print_wp(struct Location *cmd, uint8_t index) { //float t1 = (float)cmd->lat / t7; //float t2 = (float)cmd->lng / t7; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 85c07387f4..5a7e1ffe09 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -48,7 +48,7 @@ static int8_t reboot_board(uint8_t argc, const Menu::arg *argv) } // the user wants the CLI. It never exits -static void run_cli(FastSerial *port) +static void run_cli(AP_HAL::UARTDriver *port) { cliSerial = port; Menu::set_port(port); @@ -393,7 +393,7 @@ static void startup_ground(void) } // set_mode - change flight mode and perform any necessary initialisation -static void set_mode(byte mode) +static void set_mode(uint8_t mode) { // Switch to stabilize mode if requested mode requires a GPS lock if(!ap.home_is_set) {