diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index eb257f2c28..3fefe8ea04 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -17,32 +17,21 @@ // Header includes //////////////////////////////////////////////////////////////////////////////// -// AVR runtime -#include -#include -#include -#include #include +#include +#include -// Libraries -#include #include #include +#include #include #include -#include -#include // ArduPilot Mega RC Library #include // ArduPilot GPS library -#include // Wayne Truchsess I2C lib -#include // Arduino SPI lib -#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 // ArduPilot Mega polymorphic analog getter -#include // ArduPilot Mega TimerProcess #include // ArduPilot barometer library #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include #include // Inertial Sensor Library #include // ArduPilot Mega DCM Library #include // PID library @@ -50,61 +39,58 @@ #include // Range finder library #include // Filter library #include // APM FIFO Buffer -#include // Mode Filter from Filter library -#include // LowPassFilter class (inherits from Filter class) #include // APM relay #include // Photo or video camera #include #include +#include +#include +#include // MAVLink GCS definitions +#include // Camera/Antenna mount +#include // ArduPilot Mega Declination Helper Library + + // optional new controller library #if APM_CONTROL == ENABLED #include #endif +// Pre-AP_HAL compatibility +#include "compat.h" + // Configuration #include "config.h" -#include // MAVLink GCS definitions - -#include // Camera/Antenna mount - // Local modules #include "defines.h" #include "Parameters.h" #include "GCS.h" -#include // ArduPilot Mega Declination Helper Library +#include -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// 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 -#if TELEMETRY_UART2 == ENABLED -// solder bridge set to enable UART2 instead of USB MUX -FastSerialPort2(Serial3); +AP_HAL::BetterStream* cliSerial; + +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; #else -FastSerialPort3(Serial3); // Telemetry port for APM1 +const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; #endif -// port to use for command line interface -static FastSerial *cliSerial = &Serial; +//////////////////////////////////////////////////////////////////////////////// +// AP_Param Loader +//////////////////////////////////////////////////////////////////////////////// // 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); -// Outback Challenge failsafe support +//////////////////////////////////////////////////////////////////////////////// +// Outback Challenge Failsafe Support +//////////////////////////////////////////////////////////////////////////////// #if OBC_FAILSAFE == ENABLED - #include APM_OBC obc; #endif @@ -113,32 +99,6 @@ APM_OBC obc; //////////////////////////////////////////////////////////////////////////////// static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ; - -//////////////////////////////////////////////////////////////////////////////// -// ISR Registry -//////////////////////////////////////////////////////////////////////////////// -Arduino_Mega_ISR_Registry isr_registry; - - -//////////////////////////////////////////////////////////////////////////////// -// APM_RC_Class Instance -//////////////////////////////////////////////////////////////////////////////// -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -APM_RC_APM2 APM_RC; -#else -APM_RC_APM1 APM_RC; -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Dataflash -//////////////////////////////////////////////////////////////////////////////// -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -DataFlash_APM2 DataFlash; -#else -DataFlash_APM1 DataFlash; -#endif - - //////////////////////////////////////////////////////////////////////////////// // Parameters //////////////////////////////////////////////////////////////////////////////// @@ -147,7 +107,6 @@ DataFlash_APM1 DataFlash; // static Parameters g; - //////////////////////////////////////////////////////////////////////////////// // prototypes static void update_events(void); @@ -201,22 +160,22 @@ static AP_Compass_HMC5843 compass; // 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); @@ -252,32 +211,30 @@ AP_AHRS_HIL ahrs(&ins, g_gps); #error Unrecognised HIL_MODE setting. #endif // HIL MODE -// we always have a timer scheduler -AP_TimerProcess timer_scheduler; - - //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// -// GCS_MAVLINK gcs0; GCS_MAVLINK gcs3; //////////////////////////////////////////////////////////////////////////////// -// PITOT selection +// Analog Inputs //////////////////////////////////////////////////////////////////////////////// -// -#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC -AP_AnalogSource_ADC pitot_analog_source( &adc, - CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); -#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN -AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0); -#endif +AP_HAL::AnalogSource *pitot_analog_source; // a pin for reading the receiver RSSI voltage. The scaling by 0.25 // is to take the 0 to 1024 range down to an 8 bit range for MAVLink -AP_AnalogSource_Arduino RSSI_pin(-1, 0.25); +AP_HAL::AnalogSource *rssi_analog_source; + +AP_HAL::AnalogSource *vcc_pin; + +AP_HAL::AnalogSource * batt_volt_pin; +AP_HAL::AnalogSource * batt_curr_pin; + +//////////////////////////////////////////////////////////////////////////////// +// Relay +//////////////////////////////////////////////////////////////////////////////// AP_Relay relay; @@ -314,7 +271,7 @@ static bool usb_connected; enum FlightMode control_mode = INITIALISING; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch -byte oldSwitchPosition; +uint8_t oldSwitchPosition; // This is used to enable the inverted flight feature bool inverted_flight = false; // These are trim values used for elevon control @@ -341,7 +298,7 @@ static int16_t failsafe; static bool ch3_failsafe; // A timer used to help recovery from unusual attitudes. If we enter an unusual attitude // while in autonomous flight this variable is used to hold roll at 0 for a recovery period -static byte crash_timer; +static uint8_t crash_timer; // A timer used to track how long since we have received the last GCS heartbeat or rc override message static uint32_t rc_override_fs_timer = 0; @@ -367,7 +324,7 @@ static const float t7 = 10000000.0; // We use atan2 and other trig techniques to calaculate angles // A counter used to count down valid gps fixes to allow the gps estimate to settle // before recording our home position (and executing a ground start if we booted with an air start) -static byte ground_start_count = 5; +static uint8_t ground_start_count = 5; // Used to compute a speed estimate from the first valid gps fixes to decide if we are // on the ground or in the air. Used to decide if a ground start is appropriate if we // booted with an air start. @@ -401,12 +358,12 @@ static int32_t hold_course = -1; // deg * 100 dir // There may be two active commands in Auto mode. // This indicates the active navigation command by index number -static byte nav_command_index; +static uint8_t nav_command_index; // This indicates the active non-navigation command by index number -static byte non_nav_command_index; +static uint8_t non_nav_command_index; // This is the command type (eg navigate to waypoint) of the active navigation command -static byte nav_command_ID = NO_COMMAND; -static byte non_nav_command_ID = NO_COMMAND; +static uint8_t nav_command_ID = NO_COMMAND; +static uint8_t non_nav_command_ID = NO_COMMAND; //////////////////////////////////////////////////////////////////////////////// // Airspeed @@ -473,7 +430,7 @@ static float current_total1; //////////////////////////////////////////////////////////////////////////////// // Airspeed Sensors //////////////////////////////////////////////////////////////////////////////// -AP_Airspeed airspeed(&pitot_analog_source); +AP_Airspeed airspeed; //////////////////////////////////////////////////////////////////////////////// // Altitude Sensor variables @@ -645,16 +602,16 @@ static uint16_t mainLoop_count; static uint32_t medium_loopTimer_ms; // Counters for branching from main control loop to slower loops -static byte medium_loopCounter; +static uint8_t medium_loopCounter; // Number of milliseconds used in last medium loop cycle static uint8_t delta_ms_medium_loop; // Counters for branching from medium control loop to slower loops -static byte slow_loopCounter; +static uint8_t slow_loopCounter; // Counter to trigger execution of very low rate processes -static byte superslow_loopCounter; +static uint8_t superslow_loopCounter; // Counter to trigger execution of 1 Hz processes -static byte counter_one_herz; +static uint8_t counter_one_herz; // % MCU cycles used static float load; @@ -683,6 +640,21 @@ AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); //////////////////////////////////////////////////////////////////////////////// void setup() { + cliSerial = hal.console; + rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 0.25); + +#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC + pitot_analog_source = new AP_ADC_AnalogSource( &adc, + CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); +#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN + pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0); +#endif + vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); + + batt_volt_pin = hal.analogin->channel(g.battery_volt_pin); + batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); + + airspeed.init(pitot_analog_source); memcheck_init(); init_ardupilot(); } @@ -1248,3 +1220,5 @@ static void update_alt() //if(medium_loopCounter == 0 && slow_loopCounter == 0) // add_altitude_data(millis() / 100, g_gps->altitude / 10); } + +AP_HAL_MAIN(); diff --git a/ArduPlane/Arduino.h b/ArduPlane/Arduino.h new file mode 100644 index 0000000000..5c2de86bb0 --- /dev/null +++ b/ArduPlane/Arduino.h @@ -0,0 +1,3 @@ +/* Stub Arduino.h header for use with AP_HAL. (The preprocessor will put + * #include "Arduino.h" on top no matter what, but we dont have the Arduino + * core in the compiler's path to find one.) */ diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index be5fa49c1e..cf4b328dd0 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -358,8 +358,8 @@ static void set_servos(void) g.channel_roll.radio_out = g.channel_roll.radio_in; g.channel_pitch.radio_out = g.channel_pitch.radio_in; } else { - g.channel_roll.radio_out = APM_RC.InputCh(CH_ROLL); - g.channel_pitch.radio_out = APM_RC.InputCh(CH_PITCH); + g.channel_roll.radio_out = hal.rcin->read(CH_ROLL); + g.channel_pitch.radio_out = hal.rcin->read(CH_PITCH); } g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in; @@ -503,10 +503,10 @@ static void set_servos(void) #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS // send values to the PWM timers for output // ---------------------------------------- - APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos + hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos + hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos + hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos + hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos // Route configurable aux. functions to their respective servos g.rc_5.output_ch(CH_5); g.rc_6.output_ch(CH_6); @@ -522,17 +522,17 @@ static void set_servos(void) static bool demoing_servos; -static void demo_servos(byte i) { +static void demo_servos(uint8_t i) { while(i > 0) { gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); demoing_servos = true; #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS - APM_RC.OutputCh(1, 1400); + hal.rcout->write(1, 1400); mavlink_delay(400); - APM_RC.OutputCh(1, 1600); + hal.rcout->write(1, 1600); mavlink_delay(200); - APM_RC.OutputCh(1, 1500); + hal.rcout->write(1, 1500); #endif demoing_servos = false; mavlink_delay(400); diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 637a70112c..9a64814b75 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -7,10 +7,9 @@ #ifndef __GCS_H #define __GCS_H -#include +#include #include #include -#include #include /// @@ -40,7 +39,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; } @@ -87,7 +86,7 @@ public: protected: /// The stream we are communicating over - FastSerial * _port; + AP_HAL::UARTDriver *_port; }; // @@ -106,7 +105,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); @@ -137,6 +136,8 @@ public: // messages don't block the CPU mavlink_statustext_t pending_status; + // call to reset the timeout window for entering the cli + void reset_cli_timeout(); private: void handleMessage(mavlink_message_t * msg); @@ -209,6 +210,10 @@ private: // number of extra ticks to add to slow things down for the radio uint8_t stream_slowdown; + + // millis value to calculate cli timeout relative to. + // exists so we can separate the cli entry time from the system start time + uint32_t _cli_timeout; }; #endif // __GCS_H diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index ebf8d7bfe8..fad5a44b63 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -315,14 +315,14 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan) chan, millis(), 0, // port - APM_RC.InputCh(CH_1), - APM_RC.InputCh(CH_2), - APM_RC.InputCh(CH_3), - APM_RC.InputCh(CH_4), - APM_RC.InputCh(CH_5), - APM_RC.InputCh(CH_6), - APM_RC.InputCh(CH_7), - APM_RC.InputCh(CH_8), + hal.rcin->read(CH_1), + hal.rcin->read(CH_2), + hal.rcin->read(CH_3), + hal.rcin->read(CH_4), + hal.rcin->read(CH_5), + hal.rcin->read(CH_6), + hal.rcin->read(CH_7), + hal.rcin->read(CH_8), receiver_rssi); } @@ -333,16 +333,16 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) chan, micros(), 0, // port - APM_RC.OutputCh_current(0), - APM_RC.OutputCh_current(1), - APM_RC.OutputCh_current(2), - APM_RC.OutputCh_current(3), - APM_RC.OutputCh_current(4), - APM_RC.OutputCh_current(5), - APM_RC.OutputCh_current(6), - APM_RC.OutputCh_current(7)); + hal.rcout->read(0), + hal.rcout->read(1), + hal.rcout->read(2), + hal.rcout->read(3), + hal.rcout->read(4), + hal.rcout->read(5), + hal.rcout->read(6), + hal.rcout->read(7)); #else - extern RC_Channel* rc_ch[NUM_CHANNELS]; + extern RC_Channel* rc_ch[8]; mavlink_msg_servo_output_raw_send( chan, micros(), @@ -460,7 +460,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) #ifdef DESKTOP_BUILD 0); #else - I2c.lockup_count()); + hal.i2c->lockup_count()); #endif } @@ -762,10 +762,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 == (AP_HAL::BetterStream*)hal.uartA) { mavlink_comm_0_port = port; chan = MAVLINK_COMM_0; }else{ @@ -773,6 +773,7 @@ GCS_MAVLINK::init(FastSerial * port) chan = MAVLINK_COMM_1; } _queued_parameter = NULL; + reset_cli_timeout(); } void @@ -791,7 +792,7 @@ GCS_MAVLINK::update(void) #if CLI_ENABLED == ENABLED /* allow CLI to be started by hitting enter 3 times, if no * heartbeat packets have been received */ - if (mavlink_active == 0 && millis() < 20000) { + if (mavlink_active == 0 && (millis() - _cli_timeout) < 30000) { if (c == '\n' || c == '\r') { crlf_count++; } else { @@ -1117,8 +1118,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_SET_SERVO: - APM_RC.enable_out(packet.param1 - 1); - APM_RC.OutputCh(packet.param1 - 1, packet.param2); + hal.rcout->enable_ch(packet.param1 - 1); + hal.rcout->write(packet.param1 - 1, packet.param2); result = MAV_RESULT_ACCEPTED; break; @@ -1768,7 +1769,9 @@ mission_failed: v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; - rc_override_active = APM_RC.setHIL(v); + + hal.rcin->set_overrides(v, 8); + rc_override_fs_timer = millis(); break; } @@ -1990,48 +1993,40 @@ GCS_MAVLINK::queued_waypoint_send() } } +void GCS_MAVLINK::reset_cli_timeout() { + _cli_timeout = millis(); +} /* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow * MAVLink to process packets while waiting for the initialisation to * complete */ -static void mavlink_delay(unsigned long t) +static void mavlink_delay_cb() { - uint32_t tstart; static uint32_t last_1hz, last_50hz, last_5s; - - if (in_mavlink_delay) { - // this should never happen, but let's not tempt fate by - // letting the stack grow too much - delay(t); - return; - } + if (!gcs0.initialised) return; in_mavlink_delay = true; - tstart = millis(); - do { - uint32_t tnow = millis(); - if (tnow - last_1hz > 1000) { - last_1hz = tnow; - gcs_send_message(MSG_HEARTBEAT); - gcs_send_message(MSG_EXTENDED_STATUS1); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs_update(); - gcs_data_stream_send(); - } - if (tnow - last_5s > 5000) { - last_5s = tnow; - gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); - } - delay(1); + uint32_t tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs_send_message(MSG_HEARTBEAT); + gcs_send_message(MSG_EXTENDED_STATUS1); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs_update(); + gcs_data_stream_send(); + } + if (tnow - last_5s > 5000) { + last_5s = tnow; + gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); + } #if USB_MUX_PIN > 0 - check_usb_mux(); + check_usb_mux(); #endif - } while (millis() - tstart < t); in_mavlink_delay = false; } diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index fc9c2ed36d..cd14839c19 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -2,7 +2,7 @@ #if LOGGING_ENABLED == ENABLED -// Code to Write and Read packets from DataFlash log memory +// Code to Write and Read packets from hal.dataflash->log memory // Code to interact with the user to dump or erase logs #define HEAD_BYTE1 0xA3 // Decimal 163 @@ -51,20 +51,20 @@ print_log_menu(void) int16_t log_start; int16_t log_end; int16_t temp; - int16_t last_log_num = DataFlash.find_last_log(); + int16_t last_log_num = hal.dataflash->find_last_log(); - uint16_t num_logs = DataFlash.get_num_logs(); + uint16_t num_logs = hal.dataflash->get_num_logs(); - cliSerial->printf_P(PSTR("logs enabled: ")); + cliSerial->println_P(PSTR("logs enabled: ")); if (0 == g.log_bitmask) { - cliSerial->printf_P(PSTR("none")); + cliSerial->println_P(PSTR("none")); }else{ // Macro to make the following code a bit easier on the eye. // Pass it the capitalised name of the log option, as defined // in defines.h but without the LOG_ prefix. It will check for // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(# _s)) + #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(# _s)) PLOG(ATTITUDE_FAST); PLOG(ATTITUDE_MED); PLOG(GPS); @@ -88,7 +88,7 @@ print_log_menu(void) for(int16_t i=num_logs; i>=1; i--) { int16_t last_log_start = log_start, last_log_end = log_end; temp = last_log_num-i+1; - DataFlash.get_log_boundaries(temp, log_start, log_end); + hal.dataflash->get_log_boundaries(temp, log_start, log_end); cliSerial->printf_P(PSTR("Log %d, start %d, end %d\n"), (int)temp, (int)log_start, (int)log_end); if (last_log_start == log_start && last_log_end == log_end) { // we are printing bogus logs @@ -110,26 +110,29 @@ dump_log(uint8_t argc, const Menu::arg *argv) // check that the requested log number can be read dump_log = argv[1].i; - last_log_num = DataFlash.find_last_log(); + last_log_num = hal.dataflash->find_last_log(); if (dump_log == -2) { - for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) { - DataFlash.StartRead(count); + for(uint16_t count=1; count<=hal.dataflash->num_pages(); count++) { + hal.dataflash->start_read(count); cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), (int)count); - cliSerial->printf_P(PSTR("%d,\t"), (int)DataFlash.GetFileNumber()); - cliSerial->printf_P(PSTR("%d\n"), (int)DataFlash.GetFilePage()); + cliSerial->printf_P(PSTR("%d,\t"), (int)hal.dataflash->get_file()); + cliSerial->printf_P(PSTR("%d\n"), (int)hal.dataflash->get_file_page()); } return(-1); } else if (dump_log <= 0) { cliSerial->printf_P(PSTR("dumping all\n")); - Log_Read(1, DataFlash.df_NumPages); + Log_Read(1, hal.dataflash->num_pages()); return(-1); - } else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) { + } else if ((argc != 2) + || (dump_log <= (last_log_num - hal.dataflash->get_num_logs())) + || (dump_log > last_log_num)) + { cliSerial->printf_P(PSTR("bad log number\n")); return(-1); } - DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end); + hal.dataflash->get_log_boundaries(dump_log, dump_log_start, dump_log_end); cliSerial->printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"), (int)dump_log, (int)dump_log_start, @@ -143,7 +146,7 @@ dump_log(uint8_t argc, const Menu::arg *argv) static void do_erase_logs(void) { gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs")); - DataFlash.EraseAll(mavlink_delay); + hal.dataflash->erase_all(); gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete")); } @@ -212,61 +215,61 @@ process_logs(uint8_t argc, const Menu::arg *argv) // Write an attitude packet. Total length : 10 bytes static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - DataFlash.WriteInt(log_roll); - DataFlash.WriteInt(log_pitch); - DataFlash.WriteInt(log_yaw); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_ATTITUDE_MSG); + hal.dataflash->write_word(log_roll); + hal.dataflash->write_word(log_pitch); + hal.dataflash->write_word(log_yaw); + hal.dataflash->write_byte(END_BYTE); } // Write a performance monitoring packet. Total length : 19 bytes static void Log_Write_Performance() { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteLong(millis()- perf_mon_timer); - DataFlash.WriteInt((int16_t)mainLoop_count); - DataFlash.WriteInt(G_Dt_max); - DataFlash.WriteByte(0); - DataFlash.WriteByte(0); - DataFlash.WriteByte(ahrs.renorm_range_count); - DataFlash.WriteByte(ahrs.renorm_blowup_count); - DataFlash.WriteByte(gps_fix_count); - DataFlash.WriteInt(1); // AHRS health - DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000)); - DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000)); - DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000)); - DataFlash.WriteInt(pmTest1); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_PERFORMANCE_MSG); + hal.dataflash->write_dword(millis()- perf_mon_timer); + hal.dataflash->write_word((int16_t)mainLoop_count); + hal.dataflash->write_word(G_Dt_max); + hal.dataflash->write_byte(0); + hal.dataflash->write_byte(0); + hal.dataflash->write_byte(ahrs.renorm_range_count); + hal.dataflash->write_byte(ahrs.renorm_blowup_count); + hal.dataflash->write_byte(gps_fix_count); + hal.dataflash->write_word(1); // AHRS health + hal.dataflash->write_word((int)(ahrs.get_gyro_drift().x * 1000)); + hal.dataflash->write_word((int)(ahrs.get_gyro_drift().y * 1000)); + hal.dataflash->write_word((int)(ahrs.get_gyro_drift().z * 1000)); + hal.dataflash->write_word(pmTest1); + hal.dataflash->write_byte(END_BYTE); } // Write a command processing packet. Total length : 19 bytes //void Log_Write_Cmd(byte num, byte id, byte p1, int32_t alt, int32_t lat, int32_t lng) -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); - DataFlash.WriteByte(LOG_CMD_MSG); - DataFlash.WriteByte(num); - DataFlash.WriteByte(wp->id); - DataFlash.WriteByte(wp->p1); - DataFlash.WriteLong(wp->alt); - DataFlash.WriteLong(wp->lat); - DataFlash.WriteLong(wp->lng); - DataFlash.WriteByte(END_BYTE); -} - -static void Log_Write_Startup(byte type) + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_CMD_MSG); + hal.dataflash->write_byte(num); + hal.dataflash->write_byte(wp->id); + hal.dataflash->write_byte(wp->p1); + hal.dataflash->write_dword(wp->alt); + hal.dataflash->write_dword(wp->lat); + hal.dataflash->write_dword(wp->lng); + hal.dataflash->write_byte(END_BYTE); +} + +static void Log_Write_Startup(uint8_t type) { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_STARTUP_MSG); - DataFlash.WriteByte(type); - DataFlash.WriteByte(g.command_total); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_STARTUP_MSG); + hal.dataflash->write_byte(type); + hal.dataflash->write_byte(g.command_total); + hal.dataflash->write_byte(END_BYTE); // create a location struct to hold the temp Waypoints for printing struct Location cmd = get_cmd_with_index(0); @@ -284,65 +287,68 @@ static void Log_Write_Control_Tuning() { Vector3f accel = ins.get_accel(); - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - DataFlash.WriteInt(g.channel_roll.servo_out); - DataFlash.WriteInt(nav_roll_cd); - DataFlash.WriteInt((int)ahrs.roll_sensor); - DataFlash.WriteInt((int)(g.channel_pitch.servo_out)); - DataFlash.WriteInt((int)nav_pitch_cd); - DataFlash.WriteInt((int)ahrs.pitch_sensor); - DataFlash.WriteInt((int)(g.channel_throttle.servo_out)); - DataFlash.WriteInt((int)(g.channel_rudder.servo_out)); - DataFlash.WriteInt((int)(accel.y * 10000)); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_CONTROL_TUNING_MSG); + hal.dataflash->write_word(g.channel_roll.servo_out); + hal.dataflash->write_word(nav_roll_cd); + hal.dataflash->write_word((int)ahrs.roll_sensor); + hal.dataflash->write_word((int)(g.channel_pitch.servo_out)); + hal.dataflash->write_word((int)nav_pitch_cd); + hal.dataflash->write_word((int)ahrs.pitch_sensor); + hal.dataflash->write_word((int)(g.channel_throttle.servo_out)); + hal.dataflash->write_word((int)(g.channel_rudder.servo_out)); + hal.dataflash->write_word((int)(accel.y * 10000)); + hal.dataflash->write_byte(END_BYTE); } // Write a navigation tuning packet. Total length : 18 bytes static void Log_Write_Nav_Tuning() { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_NAV_TUNING_MSG); - DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); - DataFlash.WriteInt((int16_t)wp_distance); - DataFlash.WriteInt(target_bearing_cd); - DataFlash.WriteInt(nav_bearing_cd); - DataFlash.WriteInt(altitude_error_cm); - DataFlash.WriteInt((int16_t)airspeed.get_airspeed_cm()); - DataFlash.WriteInt(0); // was nav_gain_scaler - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_NAV_TUNING_MSG); + hal.dataflash->write_word((uint16_t)ahrs.yaw_sensor); + hal.dataflash->write_word((int16_t)wp_distance); + hal.dataflash->write_word(target_bearing_cd); + hal.dataflash->write_word(nav_bearing_cd); + hal.dataflash->write_word(altitude_error_cm); + hal.dataflash->write_word((int16_t)airspeed.get_airspeed_cm()); + hal.dataflash->write_word(0); // was nav_gain_scaler + hal.dataflash->write_byte(END_BYTE); } // Write a mode packet. Total length : 5 bytes -static void Log_Write_Mode(byte mode) +static void Log_Write_Mode(uint8_t mode) { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_MODE_MSG); - DataFlash.WriteByte(mode); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_MODE_MSG); + hal.dataflash->write_byte(mode); + hal.dataflash->write_byte(END_BYTE); } // Write an GPS packet. Total length : 30 bytes -static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt, - int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) +static void Log_Write_GPS( + int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, + int32_t log_gps_alt, int32_t log_mix_alt, + int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, + uint8_t log_NumSats) { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_GPS_MSG); - DataFlash.WriteLong(log_Time); - DataFlash.WriteByte(log_Fix); - DataFlash.WriteByte(log_NumSats); - DataFlash.WriteLong(log_Lattitude); - DataFlash.WriteLong(log_Longitude); - DataFlash.WriteInt(0); // was sonar_alt - DataFlash.WriteLong(log_mix_alt); - DataFlash.WriteLong(log_gps_alt); - DataFlash.WriteLong(log_Ground_Speed); - DataFlash.WriteLong(log_Ground_Course); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_GPS_MSG); + hal.dataflash->write_dword(log_Time); + hal.dataflash->write_byte(log_Fix); + hal.dataflash->write_byte(log_NumSats); + hal.dataflash->write_dword(log_Lattitude); + hal.dataflash->write_dword(log_Longitude); + hal.dataflash->write_word(0); // was sonar_alt + hal.dataflash->write_dword(log_mix_alt); + hal.dataflash->write_dword(log_gps_alt); + hal.dataflash->write_dword(log_Ground_Speed); + hal.dataflash->write_dword(log_Ground_Course); + hal.dataflash->write_byte(END_BYTE); } // Write an raw accel/gyro data packet. Total length : 28 bytes @@ -352,40 +358,40 @@ static void Log_Write_Raw() Vector3f accel = ins.get_accel(); gyro *= t7; // Scale up for storage as long integers accel *= t7; - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_RAW_MSG); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_RAW_MSG); - DataFlash.WriteLong((long)gyro.x); - DataFlash.WriteLong((long)gyro.y); - DataFlash.WriteLong((long)gyro.z); - DataFlash.WriteLong((long)accel.x); - DataFlash.WriteLong((long)accel.y); - DataFlash.WriteLong((long)accel.z); + hal.dataflash->write_dword((long)gyro.x); + hal.dataflash->write_dword((long)gyro.y); + hal.dataflash->write_dword((long)gyro.z); + hal.dataflash->write_dword((long)accel.x); + hal.dataflash->write_dword((long)accel.y); + hal.dataflash->write_dword((long)accel.z); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(END_BYTE); } static void Log_Write_Current() { - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CURRENT_MSG); - DataFlash.WriteInt(g.channel_throttle.control_in); - DataFlash.WriteInt((int)(battery_voltage1 * 100.0)); - DataFlash.WriteInt((int)(current_amps1 * 100.0)); - DataFlash.WriteInt((int)current_total1); - DataFlash.WriteByte(END_BYTE); + hal.dataflash->write_byte(HEAD_BYTE1); + hal.dataflash->write_byte(HEAD_BYTE2); + hal.dataflash->write_byte(LOG_CURRENT_MSG); + hal.dataflash->write_word(g.channel_throttle.control_in); + hal.dataflash->write_word((int)(battery_voltage1 * 100.0)); + hal.dataflash->write_word((int)(current_amps1 * 100.0)); + hal.dataflash->write_word((int)current_total1); + hal.dataflash->write_byte(END_BYTE); } // Read a Current packet static void Log_Read_Current() { cliSerial->printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"), - (int)DataFlash.ReadInt(), - ((float)DataFlash.ReadInt() / 100.f), - ((float)DataFlash.ReadInt() / 100.f), - (int)DataFlash.ReadInt()); + (int)hal.dataflash->read_word(), + ((float)hal.dataflash->read_word() / 100.f), + ((float)hal.dataflash->read_word() / 100.f), + (int)hal.dataflash->read_word()); } // Read an control tuning packet @@ -395,7 +401,7 @@ static void Log_Read_Control_Tuning() cliSerial->printf_P(PSTR("CTUN:")); for (int16_t y = 1; y < 10; y++) { - logvar = DataFlash.ReadInt(); + logvar = hal.dataflash->read_word(); if(y < 8) logvar = logvar/100.f; if(y == 9) logvar = logvar/10000.f; cliSerial->print(logvar); @@ -409,7 +415,7 @@ static void Log_Read_Nav_Tuning() { int16_t d[7]; for (int8_t i=0; i<7; i++) { - d[i] = DataFlash.ReadInt(); + d[i] = hal.dataflash->read_word(); } cliSerial->printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n d[0]/100.0, @@ -428,14 +434,14 @@ static void Log_Read_Performance() int16_t logvar; cliSerial->printf_P(PSTR("PM:")); - pm_time = DataFlash.ReadLong(); + pm_time = hal.dataflash->read_dword(); cliSerial->print(pm_time); print_comma(); for (int16_t y = 1; y <= 12; y++) { if(y < 3 || y > 7) { - logvar = DataFlash.ReadInt(); + logvar = hal.dataflash->read_word(); }else{ - logvar = DataFlash.ReadByte(); + logvar = hal.dataflash->read_byte(); } cliSerial->print(logvar); print_comma(); @@ -446,17 +452,17 @@ static void Log_Read_Performance() // Read a command processing packet static void Log_Read_Cmd() { - byte logvarb; + uint8_t logvarb; int32_t logvarl; cliSerial->printf_P(PSTR("CMD:")); for(int16_t i = 1; i < 4; i++) { - logvarb = DataFlash.ReadByte(); + logvarb = hal.dataflash->read_byte(); cliSerial->print(logvarb, DEC); print_comma(); } for(int16_t i = 1; i < 4; i++) { - logvarl = DataFlash.ReadLong(); + logvarl = hal.dataflash->read_dword(); cliSerial->print(logvarl, DEC); print_comma(); } @@ -465,7 +471,7 @@ static void Log_Read_Cmd() static void Log_Read_Startup() { - byte logbyte = DataFlash.ReadByte(); + uint8_t logbyte = hal.dataflash->read_byte(); if (logbyte == TYPE_AIRSTART_MSG) cliSerial->printf_P(PSTR("AIR START - ")); @@ -474,16 +480,16 @@ static void Log_Read_Startup() else cliSerial->printf_P(PSTR("UNKNOWN STARTUP - ")); - cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte()); + cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)hal.dataflash->read_byte()); } // Read an attitude packet static void Log_Read_Attitude() { int16_t d[3]; - d[0] = DataFlash.ReadInt(); - d[1] = DataFlash.ReadInt(); - d[2] = DataFlash.ReadInt(); + d[0] = hal.dataflash->read_word(); + d[1] = hal.dataflash->read_word(); + d[2] = hal.dataflash->read_word(); cliSerial->printf_P(PSTR("ATT: %d, %d, %u\n"), (int)d[0], (int)d[1], (unsigned)d[2]); @@ -493,25 +499,25 @@ static void Log_Read_Attitude() static void Log_Read_Mode() { cliSerial->printf_P(PSTR("MOD:")); - print_flight_mode(DataFlash.ReadByte()); + print_flight_mode(hal.dataflash->read_byte()); } // Read a GPS packet static void Log_Read_GPS() { int32_t l[7]; - byte b[2]; + uint8_t b[2]; int16_t i; - l[0] = DataFlash.ReadLong(); - b[0] = DataFlash.ReadByte(); - b[1] = DataFlash.ReadByte(); - l[1] = DataFlash.ReadLong(); - l[2] = DataFlash.ReadLong(); - i = DataFlash.ReadInt(); - l[3] = DataFlash.ReadLong(); - l[4] = DataFlash.ReadLong(); - l[5] = DataFlash.ReadLong(); - l[6] = DataFlash.ReadLong(); + l[0] = hal.dataflash->read_dword(); + b[0] = hal.dataflash->read_byte(); + b[1] = hal.dataflash->read_byte(); + l[1] = hal.dataflash->read_dword(); + l[2] = hal.dataflash->read_dword(); + i = hal.dataflash->read_word(); + l[3] = hal.dataflash->read_dword(); + l[4] = hal.dataflash->read_dword(); + l[5] = hal.dataflash->read_dword(); + l[6] = hal.dataflash->read_dword(); cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"), (long)l[0], (int)b[0], (int)b[1], l[1]/t7, l[2]/t7, @@ -525,14 +531,14 @@ static void Log_Read_Raw() float logvar; cliSerial->printf_P(PSTR("RAW:")); for (int16_t y = 0; y < 6; y++) { - logvar = (float)DataFlash.ReadLong() / t7; + logvar = (float)hal.dataflash->read_dword() / t7; cliSerial->print(logvar); print_comma(); } cliSerial->println(); } -// Read the DataFlash log memory : Packet Parser +// Read the hal.dataflash->log memory : Packet Parser static void Log_Read(int16_t start_page, int16_t end_page) { int16_t packet_count = 0; @@ -546,7 +552,7 @@ static void Log_Read(int16_t start_page, int16_t end_page) if(start_page > end_page) { - packet_count = Log_Read_Process(start_page, DataFlash.df_NumPages); + packet_count = Log_Read_Process(start_page, hal.dataflash->num_pages()); packet_count += Log_Read_Process(1, end_page); } else { packet_count = Log_Read_Process(start_page, end_page); @@ -555,17 +561,17 @@ static void Log_Read(int16_t start_page, int16_t end_page) cliSerial->printf_P(PSTR("Number of packets read: %d\n"), (int) packet_count); } -// Read the DataFlash log memory : Packet Parser +// Read the hal.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; - DataFlash.StartRead(start_page); + hal.dataflash->start_read(start_page); while (page < end_page && page != -1) { - data = DataFlash.ReadByte(); + data = hal.dataflash->read_byte(); switch(log_step) // This is a state machine to read the packets { @@ -634,7 +640,7 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page) log_step = 0; // Restart sequence: new packet... break; } - page = DataFlash.GetPage(); + page = hal.dataflash->get_page(); } return packet_count; } @@ -642,18 +648,18 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page) #else // LOGGING_ENABLED // dummy functions -static void Log_Write_Mode(byte mode) { +static void Log_Write_Mode(uint8_t mode) { } -static void Log_Write_Startup(byte type) { +static void Log_Write_Startup(uint8_t type) { } -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_Current() { } static void Log_Write_Nav_Tuning() { } static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt, - int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) { + int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) { } static void Log_Write_Performance() { } diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 704706cf31..e577f17c1d 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -12,12 +12,18 @@ hil: hil-apm2: make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" +hil-apm2-nologging: + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DLOGGING_ENABLED=DISABLED" + hilsensors: make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS" hilsensors-apm2: make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" +hilsensors-apm2-nologging: + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DLOGGING_ENABLED=DISABLED" + hilnocli: make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED" @@ -39,6 +45,9 @@ heli: apm2: make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" +apm2-nologging: + make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DLOGGING_ENABLED=DISABLED" + apm2-uart2: make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DTELEMETRY_UART2=ENABLED" diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 02f943c393..1cebea55bf 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -577,8 +577,8 @@ static void do_set_home() static void do_set_servo() { - APM_RC.enable_out(next_nonnav_command.p1 - 1); - APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); + hal.rcout->enable_ch(next_nonnav_command.p1 - 1); + hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt); } static void do_set_relay() @@ -595,7 +595,7 @@ static void do_set_relay() static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint8_t delay_time) { - extern RC_Channel *rc_ch[NUM_CHANNELS]; + extern RC_Channel *rc_ch[8]; channel = channel - 1; if (channel < 5 || channel > 8) { // not allowed diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 6ebf43c040..e39c7a37b1 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -58,7 +58,7 @@ static void process_next_command() // and loads conditional or immediate commands if applicable struct Location temp; - byte old_index = nav_command_index; + uint8_t old_index = nav_command_index; // these are Navigation/Must commands // --------------------------------- diff --git a/ArduPlane/compat.h b/ArduPlane/compat.h new file mode 100644 index 0000000000..451e28c2c9 --- /dev/null +++ b/ArduPlane/compat.h @@ -0,0 +1,15 @@ + +#ifndef __COMPAT_H__ +#define __COMPAT_H__ + +#define OUTPUT GPIO_OUTPUT +#define INPUT GPIO_INPUT + +#define HIGH 1 +#define LOW 0 + +/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */ +static void run_cli(AP_HAL::UARTDriver *port); + +#endif // __COMPAT_H__ + diff --git a/ArduPlane/compat.pde b/ArduPlane/compat.pde new file mode 100644 index 0000000000..171d2dcf79 --- /dev/null +++ b/ArduPlane/compat.pde @@ -0,0 +1,37 @@ + + +void delay(uint32_t ms) +{ + hal.scheduler->delay(ms); +} + +void mavlink_delay(uint32_t ms) +{ + hal.scheduler->delay(ms); +} + +uint32_t millis() +{ + return hal.scheduler->millis(); +} + +uint32_t micros() +{ + return hal.scheduler->micros(); +} + +void pinMode(uint8_t pin, uint8_t output) +{ + hal.gpio->pinMode(pin, output); +} + +void digitalWrite(uint8_t pin, uint8_t out) +{ + hal.gpio->write(pin,out); +} + +uint8_t digitalRead(uint8_t pin) +{ + return hal.gpio->read(pin); +} + diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 7493e763e5..b6429ba035 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -4,7 +4,7 @@ static void read_control_switch() { static bool switch_debouncer; - byte switchPosition = readSwitch(); + uint8_t switchPosition = readSwitch(); // If switchPosition = 255 this indicates that the mode control channel input was out of range // If we get this value we do not want to change modes. @@ -18,7 +18,7 @@ static void read_control_switch() // as a spring loaded trainer switch). if (oldSwitchPosition != switchPosition || (g.reset_switch_chan != 0 && - APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { + hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { if (switch_debouncer == false) { // this ensures that mode switches only happen if the @@ -36,7 +36,7 @@ static void read_control_switch() } if (g.reset_mission_chan != 0 && - APM_RC.InputCh(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) { + hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) { // reset to first waypoint in mission prev_WP = current_loc; change_command(0); @@ -47,12 +47,12 @@ static void read_control_switch() if (g.inverted_flight_ch != 0) { // if the user has configured an inverted flight channel, then // fly upside down when that channel goes above INVERTED_FLIGHT_PWM - inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); + inverted_flight = (control_mode != MANUAL && hal.rcin->read(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); } } -static byte readSwitch(void){ - uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); +static uint8_t readSwitch(void){ + uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1); if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 901d02aae8..833d1ef1ea 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -36,7 +36,8 @@ static void failsafe_long_on_event(int16_t fstype) { // This is how to handle a long loss of control signal failsafe. gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, ")); - APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC + // If the GCS is locked up we allow control to revert to RC + hal.rcin->clear_overrides(); failsafe = fstype; switch(control_mode) { @@ -104,11 +105,11 @@ static void update_events(void) switch (event_state.type) { case EVENT_TYPE_SERVO: - APM_RC.enable_out(event_state.rc_channel); + hal.rcout->enable_ch(event_state.rc_channel); if (event_state.repeat & 1) { - APM_RC.OutputCh(event_state.rc_channel, event_state.undo_value); + hal.rcout->write(event_state.rc_channel, event_state.undo_value); } else { - APM_RC.OutputCh(event_state.rc_channel, event_state.servo_value); + hal.rcout->write(event_state.rc_channel, event_state.servo_value); } break; diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde index c5640d7e64..c493804cff 100644 --- a/ArduPlane/failsafe.pde +++ b/ArduPlane/failsafe.pde @@ -38,13 +38,13 @@ void failsafe_check(uint32_t tnow) if (in_failsafe && tnow - last_timestamp > 20000) { // pass RC inputs to outputs every 20ms last_timestamp = tnow; - APM_RC.clearOverride(); + hal.rcin->clear_overrides(); uint8_t start_ch = 0; if (demoing_servos) { start_ch = 1; } for (uint8_t ch=start_ch; ch<4; ch++) { - APM_RC.OutputCh(ch, APM_RC.InputCh(ch)); + hal.rcout->write(ch, hal.rcin->read(ch)); } RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index a93bbfdd44..916e006b39 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -22,7 +22,7 @@ static struct geofence_state { uint16_t breach_count; uint8_t breach_type; uint32_t breach_time; - byte old_switch_position; + uint8_t old_switch_position; /* point 0 is the return point */ Vector2l boundary[MAX_FENCEPOINTS]; } *geofence_state; @@ -129,7 +129,7 @@ static bool geofence_enabled(void) g.fence_total < 5 || (g.fence_action != FENCE_ACTION_REPORT && (g.fence_channel == 0 || - APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM))) { + hal.rcin->read(g.fence_channel-1) < FENCE_ENABLE_PWM))) { // geo-fencing is disabled if (geofence_state != NULL) { // re-arm for when the channel trigger is switched on diff --git a/ArduPlane/nocore.inoflag b/ArduPlane/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index b534365163..2cddb7917a 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -2,10 +2,10 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling -extern RC_Channel* rc_ch[NUM_CHANNELS]; +extern RC_Channel* rc_ch[8]; static void init_rc_in() { @@ -45,36 +45,34 @@ static void init_rc_in() static void init_rc_out() { - APM_RC.Init( &isr_registry ); // APM Radio initialization - - APM_RC.enable_out(CH_1); - APM_RC.enable_out(CH_2); - APM_RC.enable_out(CH_3); - APM_RC.enable_out(CH_4); + hal.rcout->enable_ch(CH_1); + hal.rcout->enable_ch(CH_2); + hal.rcout->enable_ch(CH_3); + hal.rcout->enable_ch(CH_4); enable_aux_servos(); // Initialization of servo outputs - APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); + hal.rcout->write(CH_1, g.channel_roll.radio_trim); + hal.rcout->write(CH_2, g.channel_pitch.radio_trim); + hal.rcout->write(CH_3, g.channel_throttle.radio_min); + hal.rcout->write(CH_4, g.channel_rudder.radio_trim); - APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); - APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); - APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + hal.rcout->write(CH_5, g.rc_5.radio_trim); + hal.rcout->write(CH_6, g.rc_6.radio_trim); + hal.rcout->write(CH_7, g.rc_7.radio_trim); + hal.rcout->write(CH_8, g.rc_8.radio_trim); #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM1 - APM_RC.OutputCh(CH_9, g.rc_9.radio_trim); - APM_RC.OutputCh(CH_10, g.rc_10.radio_trim); - APM_RC.OutputCh(CH_11, g.rc_11.radio_trim); + hal.rcout->write(CH_9, g.rc_9.radio_trim); + hal.rcout->write(CH_10, g.rc_10.radio_trim); + hal.rcout->write(CH_11, g.rc_11.radio_trim); #endif } static void read_radio() { - ch1_temp = APM_RC.InputCh(CH_ROLL); - ch2_temp = APM_RC.InputCh(CH_PITCH); + ch1_temp = hal.rcin->read(CH_ROLL); + ch2_temp = hal.rcin->read(CH_PITCH); if(g.mix_mode == 0) { g.channel_roll.set_pwm(ch1_temp); @@ -84,12 +82,12 @@ static void read_radio() g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); } - g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); - g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); - g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); - g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); - g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); + g.channel_throttle.set_pwm(hal.rcin->read(CH_3)); + g.channel_rudder.set_pwm(hal.rcin->read(CH_4)); + g.rc_5.set_pwm(hal.rcin->read(CH_5)); + g.rc_6.set_pwm(hal.rcin->read(CH_6)); + g.rc_7.set_pwm(hal.rcin->read(CH_7)); + g.rc_8.set_pwm(hal.rcin->read(CH_8)); control_failsafe(g.channel_throttle.radio_in); diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 6b09753ff8..b867bba012 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -7,7 +7,7 @@ static LowPassFilterInt32 altitude_filter; static void init_barometer(void) { gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer")); - barometer.calibrate(mavlink_delay); + barometer.calibrate(); // filter at 100ms sampling, with 0.7Hz cutoff frequency altitude_filter.set_cutoff_frequency(0.1, 0.7); @@ -33,7 +33,7 @@ static void read_airspeed(void) static void zero_airspeed(void) { - airspeed.calibrate(mavlink_delay); + airspeed.calibrate(); gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated")); } @@ -45,16 +45,14 @@ 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); // this copes with changing the pin at runtime - batt_volt_pin.set_pin(g.battery_volt_pin); - battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin.read_average()); + batt_volt_pin->set_pin(g.battery_volt_pin); + battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin->read_average()); } if(g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino batt_curr_pin(g.battery_curr_pin); // this copes with changing the pin at runtime - batt_curr_pin.set_pin(g.battery_curr_pin); - current_amps1 = CURRENT_AMPS(batt_curr_pin.read_average()); + batt_curr_pin->set_pin(g.battery_curr_pin); + current_amps1 = CURRENT_AMPS(batt_curr_pin->read_average()); current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } @@ -69,8 +67,8 @@ static void read_battery(void) // RC_CHANNELS_SCALED message void read_receiver_rssi(void) { - RSSI_pin.set_pin(g.rssi_pin); - float ret = RSSI_pin.read(); + rssi_analog_source->set_pin(g.rssi_pin); + float ret = rssi_analog_source->read_average(); receiver_rssi = constrain(ret, 0, 255); } diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 33495f4331..439e33cecb 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -170,7 +170,9 @@ setup_radio(uint8_t argc, const Menu::arg *argv) g.rc_8.update_min_max(); if(cliSerial->available() > 0) { - cliSerial->flush(); + while (cliSerial->available() > 0) { + cliSerial->read(); + } g.channel_roll.save_eeprom(); g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); @@ -192,7 +194,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte switchPosition, mode = 0; + uint8_t switchPosition, mode = 0; cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); @@ -306,34 +308,14 @@ setup_level(uint8_t argc, const Menu::arg *argv) handle full accelerometer calibration via user dialog */ -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); - va_end(arg_list); -} - -static void setup_wait_key(void) -{ - // wait for user input - while (!cliSerial->available()) { - delay(20); - } - // clear input buffer - while( cliSerial->available() ) { - cliSerial->read(); - } -} - static int8_t 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); - if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) { + flash_leds); + if (ins.calibrate_accel(flash_leds, hal.console)) { if (g.manual_level == 0) { cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1")); g.manual_level.set_and_save(1); @@ -564,7 +546,7 @@ print_radio_values() } static void -print_switch(byte p, byte m) +print_switch(uint8_t p, uint8_t m) { cliSerial->printf_P(PSTR("Pos %d: "),p); print_flight_mode(m); @@ -623,7 +605,7 @@ radio_input_switch(void) static void zero_eeprom(void) { - byte b = 0; + uint8_t b = 0; cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) { eeprom_write_byte((uint8_t *) i, b); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 1e6a32b03a..7f18f2a572 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -49,10 +49,10 @@ 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) { // disable the failsafe code in the CLI - timer_scheduler.set_failsafe(NULL); + hal.scheduler->register_timer_failsafe(NULL,1); cliSerial = port; Menu::set_port(port); @@ -90,44 +90,20 @@ static void init_ardupilot() // The console port buffers are defined to be sufficiently large to support // the MAVLink protocol efficiently // - Serial.begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE); + hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE); // GPS serial port. // // standard gps running - Serial1.begin(38400, 256, 16); + hal.uartB->begin(38400, 256, 16); 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 - // - // Initialize the ISR registry. - // - isr_registry.init(); // - // Initialize the timer scheduler to use the ISR registry. - // - - timer_scheduler.init( &isr_registry ); - - // initialise the analog port reader - AP_AnalogSource_Arduino::init_timer(&timer_scheduler); - - // - // Check the EEPROM format version before loading any parameters from EEPROM. + // Check the EEPROM format version before loading any parameters from EEPROM // load_parameters(); @@ -136,46 +112,47 @@ static void init_ardupilot() g.num_resets.set_and_save(g.num_resets+1); // init the GCS - gcs0.init(&Serial); + gcs0.init(hal.uartA); + // Register mavlink_delay_cb, which will run anytime you have + // more than 5ms remaining in your call to hal.scheduler->delay + hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); #if USB_MUX_PIN > 0 if (!usb_connected) { // we are not connected via USB, re-init UART0 with right // baud rate - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); + hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } #else // we have a 2nd serial port for telemetry - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, SERIAL_BUFSIZE); - gcs3.init(&Serial3); + hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), + 128, SERIAL_BUFSIZE); + gcs3.init(hal.uartC); #endif mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED - DataFlash.Init(); // DataFlash log initialization - if (!DataFlash.CardInserted()) { + if (!hal.dataflash->media_present()) { gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted")); g.log_bitmask.set(0); - } else if (DataFlash.NeedErase()) { + } else if (hal.dataflash->need_erase()) { gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS")); do_erase_logs(); + gcs0.reset_cli_timeout(); } if (g.log_bitmask != 0) { - DataFlash.start_new_log(); + hal.dataflash->start_new_log(); } #endif #if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_ADC == ENABLED - adc.Init(&timer_scheduler); // APM ADC library initialization + adc.Init(); // APM ADC library initialization #endif - // initialise the analog port reader - AP_AnalogSource_Arduino::init_timer(&timer_scheduler); - - barometer.init(&timer_scheduler); + barometer.init(); if (g.compass_enabled==true) { compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft @@ -207,9 +184,9 @@ static void init_ardupilot() mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id mavlink_system.type = MAV_TYPE_FIXED_WING; - rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override + // Set initial values for no override + rc_override_active = hal.rcin->set_overrides(rc_override, 8); - RC_Channel::set_apm_rc( &APM_RC ); // Provide reference to RC outputs. init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs @@ -217,7 +194,7 @@ static void init_ardupilot() pinMode(A_LED_PIN, OUTPUT); // GPS status LED pinMode(B_LED_PIN, OUTPUT); // GPS status LED #if CONFIG_RELAY == ENABLED - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + relay.init(); #endif #if FENCE_TRIGGERED_PIN > 0 @@ -229,12 +206,12 @@ static void init_ardupilot() * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ - timer_scheduler.set_failsafe(failsafe_check); + hal.scheduler->register_timer_failsafe(failsafe_check, 1000); 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 if (ENABLE_AIR_START == 1) { @@ -247,19 +224,21 @@ static void init_ardupilot() #if HIL_MODE != HIL_MODE_ATTITUDE ins.init(AP_InertialSensor::WARM_START, ins_sample_rate, - mavlink_delay, flash_leds, &timer_scheduler); + flash_leds); - ahrs.init(&timer_scheduler); + ahrs.init(); ahrs.set_fly_forward(true); #endif // This delay is important for the APM_RC library to work. // We need some time for the comm between the 328 and 1280 to be established. int old_pulse = 0; - while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(g.flight_mode_channel)) > 5 || - APM_RC.InputCh(g.flight_mode_channel) == 1000 || - APM_RC.InputCh(g.flight_mode_channel) == 1200)) { - old_pulse = APM_RC.InputCh(g.flight_mode_channel); + while (millis()<=1000 + && (abs(old_pulse - hal.rcin->read(g.flight_mode_channel)) > 5 + || hal.rcin->read(g.flight_mode_channel) == 1000 + || hal.rcin->read(g.flight_mode_channel) == 1200)) + { + old_pulse = hal.rcin->read(g.flight_mode_channel); delay(25); } g_gps->update(); @@ -324,9 +303,9 @@ static void startup_ground(void) // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly - Serial.set_blocking_writes(false); + hal.uartC->set_blocking_writes(false); if (gcs3.initialised) { - Serial3.set_blocking_writes(false); + hal.uartC->set_blocking_writes(false); } gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); @@ -438,13 +417,13 @@ static void startup_INS_ground(bool force_accel_level) ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - mavlink_delay, flash_leds, &timer_scheduler); + flash_leds); #if HIL_MODE == HIL_MODE_DISABLED if (force_accel_level || g.manual_level == 0) { // when MANUAL_LEVEL is set to 1 we don't do accelerometer // levelling on each boot, and instead rely on the user to do // it once via the ground station - ins.init_accel(mavlink_delay, flash_leds); + ins.init_accel(flash_leds); } #endif ahrs.set_fly_forward(true); @@ -541,9 +520,9 @@ static void check_usb_mux(void) // the user has switched to/from the telemetry port usb_connected = usb_check; if (usb_connected) { - Serial.begin(SERIAL0_BAUD); + hal.uartA->begin(SERIAL0_BAUD); } else { - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); + hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } } #endif @@ -564,8 +543,7 @@ void flash_leds(bool on) */ uint16_t board_voltage(void) { - static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC); - return vcc.read_vcc(); + return vcc_pin->read_latest(); } @@ -574,20 +552,7 @@ uint16_t board_voltage(void) */ static void reboot_apm(void) { - cliSerial->printf_P(PSTR("REBOOTING\n")); - delay(100); // let serial flush - // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1250663814/ - // for the method -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - // this relies on the bootloader resetting the watchdog, which - // APM1 doesn't do - cli(); - wdt_enable(WDTO_15MS); -#else - // this works on APM1 - void (*fn)(void) = NULL; - fn(); -#endif + hal.scheduler->reboot(); while (1); } @@ -630,3 +595,5 @@ static void print_comma(void) { cliSerial->print_P(PSTR(",")); } + + diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index f952e38030..f67c49b36d 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -134,12 +134,12 @@ test_passthru(uint8_t argc, const Menu::arg *argv) delay(20); // New radio frame? (we could use also if((millis()- timer) > 20) - if (APM_RC.GetState() == 1) { + if (hal.rcin->valid() > 0) { cliSerial->print_P(PSTR("CH:")); for(int16_t i = 0; i < 8; i++) { - cliSerial->print(APM_RC.InputCh(i)); // Print channel values + cliSerial->print(hal.rcin->read(i)); // Print channel values print_comma(); - APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos + hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos } cliSerial->println(); } @@ -192,7 +192,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv) { - byte fail_test; + uint8_t fail_test; print_hit_enter(); for(int16_t i = 0; i < 50; i++) { delay(20); @@ -319,7 +319,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - 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); test_wp_print(&temp, i); } @@ -328,7 +328,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) } static void -test_wp_print(struct Location *cmd, byte wp_index) +test_wp_print(struct Location *cmd, uint8_t wp_index) { cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), (int)wp_index, @@ -349,8 +349,8 @@ test_xbee(uint8_t argc, const Menu::arg *argv) while(1) { - if (Serial3.available()) - Serial3.write(Serial3.read()); + if (hal.uartC->available()) + hal.uartC->write(hal.uartC->read()); if(cliSerial->available() > 0) { return (0); @@ -371,7 +371,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv) while(1) { delay(20); - byte switchPosition = readSwitch(); + uint8_t switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition) { cliSerial->printf_P(PSTR("Position %d\n"), (int)switchPosition); oldSwitchPosition = switchPosition; @@ -389,20 +389,20 @@ static int8_t test_logging(uint8_t argc, const Menu::arg *argv) { cliSerial->println_P(PSTR("Testing dataflash logging")); - if (!DataFlash.CardInserted()) { + if (!hal.dataflash->media_present()) { cliSerial->println_P(PSTR("ERR: No dataflash inserted")); return 0; } - DataFlash.ReadManufacturerID(); + hal.dataflash->read_mfg_id(); cliSerial->printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), - (unsigned)DataFlash.df_manufacturer, - (unsigned)DataFlash.df_device); - cliSerial->printf_P(PSTR("NumPages: %u PageSize: %u\n"), - (unsigned)DataFlash.df_NumPages+1, - (unsigned)DataFlash.df_PageSize); - DataFlash.StartRead(DataFlash.df_NumPages+1); + (unsigned)hal.dataflash->mfg_id(), + (unsigned)hal.dataflash->device_id()); + cliSerial->printf_P(PSTR("NumPages: %u\n"), + (unsigned)hal.dataflash->num_pages()+1); + hal.dataflash->start_read(hal.dataflash->num_pages()+1); cliSerial->printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), - (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); + (unsigned long)hal.dataflash->read_dword(), + (unsigned long)DF_LOGGING_FORMAT); return 0; } @@ -416,13 +416,13 @@ static int8_t test_adc(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); - adc.Init(&timer_scheduler); + adc.Init(); delay(1000); cliSerial->printf_P(PSTR("ADC\n")); delay(1000); while(1) { - for (int16_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i)); + for (int8_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i)); cliSerial->println(); delay(100); if(cliSerial->available() > 0) { @@ -468,7 +468,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) //cliSerial->printf_P(PSTR("Calibrating.")); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); + flash_leds); ahrs.reset(); print_hit_enter(); @@ -531,7 +531,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) // we need the AHRS initialised for this test ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); + flash_leds); ahrs.reset(); int16_t counter = 0; @@ -603,7 +603,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { - float airspeed_ch = pitot_analog_source.read(); + float airspeed_ch = pitot_analog_source->read_average(); // cliSerial->println(pitot_analog_source.read()); cliSerial->printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch); @@ -666,14 +666,16 @@ test_rawgps(uint8_t argc, const Menu::arg *argv) delay(1000); while(1) { - if (Serial3.available()) { - digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); + // Blink Yellow LED if we are sending data to GPS + if (hal.uartC->available()) { + digitalWrite(B_LED_PIN, LED_ON); + hal.uartB->write(hal.uartC->read()); digitalWrite(B_LED_PIN, LED_OFF); } - if (Serial1.available()) { - digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); + // Blink Red LED if we are receiving data from GPS + if (hal.uartB->available()) { + digitalWrite(C_LED_PIN, LED_ON); + hal.uartC->write(hal.uartB->read()); digitalWrite(C_LED_PIN, LED_OFF); } if(cliSerial->available() > 0) {