Rover: first cut at porting rover to AP_HAL

APMrover2/APM_Config.h

@@ -3,8 +3,6 @@
 // This file is just a placeholder for your configuration file.  If you wish to change any of the setup parameters from
 // their default values, place the appropriate #define statements here.
 
-//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
-
 #define LITE  DISABLED    // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
                           // if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
 

APMrover2/APMrover2.pde

@@ -69,28 +69,19 @@ version 2.1 of the License, or (at your option) any later version.
 // Header includes
 ////////////////////////////////////////////////////////////////////////////////
 
-// AVR runtime
-#include <avr/io.h>
-#include <avr/eeprom.h>
-#include <avr/pgmspace.h>
 #include <math.h>
+#include <stdarg.h>
+#include <stdio.h>
 
 // Libraries
-#include <FastSerial.h>
 #include <AP_Common.h>
 #include <AP_Progmem.h>
+#include <AP_HAL.h>
 #include <AP_Menu.h>
 #include <AP_Param.h>
-#include <Arduino_Mega_ISR_Registry.h>
-#include <APM_RC.h>         // ArduPilot Mega RC Library
 #include <AP_GPS.h>         // ArduPilot GPS library
-#include <I2C.h>			// Wayne Truchsess I2C lib
-#include <SPI.h>			// Arduino SPI lib
-#include <AP_Semaphore.h>   // for removing conflict between optical flow and dataflash on SPI3 bus
-#include <DataFlash.h>      // ArduPilot Mega Flash Memory Library
 #include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
-#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter
+#include <AP_ADC_AnalogSource.h>
-#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
 #include <AP_Baro.h>
 #include <AP_Compass.h>     // ArduPilot Mega Magnetometer Library
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
@@ -108,6 +99,14 @@ version 2.1 of the License, or (at your option) any later version.
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
 #include <AP_Airspeed.h>    // needed for AHRS build
 #include <memcheck.h>
+#include <DataFlash.h>
+#include <SITL.h>
+#include <stdarg.h>
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include <AP_HAL_Empty.h>
+#include "compat.h"
 
 // Configuration
 #include "config.h"
@@ -119,24 +118,9 @@ version 2.1 of the License, or (at your option) any later version.
 
 #include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
 
-////////////////////////////////////////////////////////////////////////////////
+AP_HAL::BetterStream* cliSerial;
-// 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);
-#else
- FastSerialPort3(Serial3);       // Telemetry port for APM1
-#endif
 
-static FastSerial *cliSerial = &Serial;
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
 // this sets up the parameter table, and sets the default values. This
 // must be the first AP_Param variable declared to ensure its
@@ -144,31 +128,6 @@ static FastSerial *cliSerial = &Serial;
 // variables
 AP_Param param_loader(var_info, WP_START_BYTE);
 
-////////////////////////////////////////////////////////////////////////////////
-// 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
-AP_Semaphore spi3_semaphore;
-DataFlash_APM2 DataFlash(&spi3_semaphore);
-#else
-DataFlash_APM1 DataFlash;
-#endif
-
 ////////////////////////////////////////////////////////////////////////////////
 // the rate we run the main loop at
 ////////////////////////////////////////////////////////////////////////////////
@@ -187,6 +146,17 @@ static Parameters      g;
 // prototypes
 static void update_events(void);
 
+////////////////////////////////////////////////////////////////////////////////
+// DataFlash
+////////////////////////////////////////////////////////////////////////////////
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+DataFlash_APM1 DataFlash;
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+DataFlash_APM2 DataFlash;
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+DataFlash_SITL DataFlash;
+#endif
+
 
 ////////////////////////////////////////////////////////////////////////////////
 // Sensors
@@ -214,36 +184,34 @@ static AP_Int8		*flight_modes = &g.flight_mode1;
 static AP_ADC_ADS7844          adc;
 #endif
 
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-AP_Baro_BMP085_HIL barometer;
+static AP_Compass_HIL compass;
-AP_Compass_HIL compass;
+static SITL sitl;
-#include <SITL.h>
-SITL sitl;
 #else
 static AP_Compass_HMC5843      compass;
 #endif
 
 // real GPS selection
 #if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
-AP_GPS_Auto     g_gps_driver(&Serial1, &g_gps);
+AP_GPS_Auto     g_gps_driver(&g_gps);
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
-AP_GPS_NMEA     g_gps_driver(&Serial1);
+AP_GPS_NMEA     g_gps_driver();
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
-AP_GPS_SIRF     g_gps_driver(&Serial1);
+AP_GPS_SIRF     g_gps_driver();
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
-AP_GPS_UBLOX    g_gps_driver(&Serial1);
+AP_GPS_UBLOX    g_gps_driver();
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
-AP_GPS_MTK      g_gps_driver(&Serial1);
+AP_GPS_MTK      g_gps_driver();
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
-AP_GPS_MTK16    g_gps_driver(&Serial1);
+AP_GPS_MTK16    g_gps_driver();
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
-AP_GPS_None     g_gps_driver(NULL);
+AP_GPS_None     g_gps_driver();
 
 #else
  #error Unrecognised GPS_PROTOCOL setting.
@@ -251,7 +219,9 @@ AP_GPS_None     g_gps_driver(NULL);
 
 # if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
   AP_InertialSensor_MPU6000 ins;
-# else
+# elif CONFIG_INS_TYPE == CONFIG_INS_SITL
+  AP_InertialSensor_Stub ins;
+#else
   AP_InertialSensor_Oilpan ins( &adc );
 #endif // CONFIG_INS_TYPE
 
@@ -274,9 +244,6 @@ AP_Compass_HIL          compass; // never used
  #error Unrecognised HIL_MODE setting.
 #endif // HIL MODE
 
-// we always have a timer scheduler
-AP_TimerProcess timer_scheduler;
-
 ////////////////////////////////////////////////////////////////////////////////
 // GCS selection
 ////////////////////////////////////////////////////////////////////////////////
@@ -284,22 +251,23 @@ AP_TimerProcess timer_scheduler;
 GCS_MAVLINK	gcs0;
 GCS_MAVLINK	gcs3;
 
+// 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_HAL::AnalogSource *rssi_analog_source;
+
+AP_HAL::AnalogSource *vcc_pin;
+
+AP_HAL::AnalogSource * batt_volt_pin;
+AP_HAL::AnalogSource * batt_curr_pin;
+
 ////////////////////////////////////////////////////////////////////////////////
 // SONAR selection
 ////////////////////////////////////////////////////////////////////////////////
 //
 ModeFilterInt16_Size5 sonar_mode_filter(2);
 #if CONFIG_SONAR == ENABLED
-/*
+    AP_HAL::AnalogSource *sonar_analog_source;
-	#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
+    AP_RangeFinder_MaxsonarXL *sonar;
-	AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
-	#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
-		AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
-	#endif
-	AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
-*/
-    AP_AnalogSource_Arduino sonar_analog_source(A0);   // use AN0 analog pin for APM2 on left
-    AP_RangeFinder_SharpGP2Y sonar(&sonar_analog_source, &sonar_mode_filter);
 #endif
 
 // relay support
@@ -341,10 +309,10 @@ static const char *comma = ",";
 ////////////////////////////////////////////////////////////////////////////////
 // This is the state of the flight control system
 // There are multiple states defined such as MANUAL, FBW-A, AUTO
-byte    control_mode        = INITIALISING;
+uint8_t    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;
 // These are values received from the GCS if the user is using GCS joystick
 // control and are substituted for the values coming from the RC radio
 static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
@@ -383,7 +351,7 @@ static const 	float t7			= 10000000.0;
 
 // 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.
@@ -416,12 +384,12 @@ static bool rtl_complete = false;
 
 // 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 uint8_t	nav_command_ID		= NO_COMMAND;	
-static byte	non_nav_command_ID	= NO_COMMAND;	
+static uint8_t	non_nav_command_ID	= NO_COMMAND;	
 
 static float	groundspeed_error;	
 // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
@@ -455,7 +423,7 @@ static float	crosstrack_error;
 // Used to track the CH7 toggle state.
 // When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
 // This allows advanced functionality to know when to execute
-static boolean trim_flag;
+static bool trim_flag;
 // This register tracks the current Mission Command index when writing
 // a mission using CH7 in flight
 static int8_t CH7_wp_index;
@@ -500,7 +468,7 @@ static int32_t wp_totalDistance;
 // repeating event control
 ////////////////////////////////////////////////////////////////////////////////
 // Flag indicating current event type
-static byte 		event_id;
+static uint8_t 		event_id;
 // when the event was started in ms
 static int32_t 		event_timer;
 // how long to delay the next firing of event in millis
@@ -582,16 +550,16 @@ static uint16_t			mainLoop_count;
 // Time in miliseconds of start of medium control loop.  Milliseconds
 static uint32_t 	medium_loopTimer;
 // 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;
@@ -602,6 +570,27 @@ static float 			load;
 
 void setup() {
 	memcheck_init();
+    cliSerial = hal.console;
+
+    rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 0.25);
+    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);
+
+#if CONFIG_SONAR == ENABLED
+ #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
+    sonar_analog_source = new AP_ADC_AnalogSource(
+        &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
+ #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
+    sonar_analog_source = hal.analogin->channel(
+        CONFIG_SONAR_SOURCE_ANALOG_PIN);
+ #else
+  #warning "Invalid CONFIG_SONAR_SOURCE"
+ #endif
+    sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
+                                          &sonar_mode_filter);
+#endif
+
 	init_ardupilot();
 }
 
@@ -688,7 +677,7 @@ static void fast_loop()
 	// ----------
 #if CONFIG_SONAR == ENABLED
 	if(g.sonar_enabled){
-		sonar_dist = sonar.read();
+		sonar_dist = sonar->read();
 
 	if(sonar_dist <= g.sonar_trigger)  {  // obstacle detected in front 
             obstacle = true;
@@ -972,3 +961,5 @@ static void update_navigation()
         break;
 	}
 }
+
+AP_HAL_MAIN();

APMrover2/Attitude.pde

@@ -120,10 +120,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
+    hal.rcout->write(CH_1, g.channel_roll.radio_out);     // send to Servos
-	APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
+    hal.rcout->write(CH_2, g.channel_pitch.radio_out);     // send to Servos
-	APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
+    hal.rcout->write(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_4, g.channel_rudder.radio_out);     // send to Servos
 	// Route configurable aux. functions to their respective servos
 
 	g.rc_5.output_ch(CH_5);
@@ -134,18 +134,22 @@ static void set_servos(void)
 #endif
 }
 
-static void demo_servos(byte i) {
+static bool demoing_servos;
 
-	while(i > 0){
+static void demo_servos(uint8_t i) {
-		gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
+
+    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);
+        mavlink_delay(400);
-		APM_RC.OutputCh(1, 1600);
+        hal.rcout->write(1, 1600);
-		mavlink_delay(200);
+        mavlink_delay(200);
-		APM_RC.OutputCh(1, 1500);
+        hal.rcout->write(1, 1500);
 #endif
-		mavlink_delay(400);
+        demoing_servos = false;
-		i--;
+        mavlink_delay(400);
-	}
+        i--;
+    }
 }

APMrover2/GCS.h

@@ -6,10 +6,8 @@
 #ifndef __GCS_H
 #define __GCS_H
 
-#include <FastSerial.h>
 #include <AP_Common.h>
 #include <GPS.h>
-#include <Stream.h>
 #include <stdint.h>
 
 ///
@@ -39,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;
@@ -61,19 +59,12 @@ public:
 	///
 	void		send_message(enum ap_message id) {}
 
-	/// Send a text message.
-	///
-	/// @param	severity	A value describing the importance of the message.
-	/// @param	str			The text to be sent.
-	///
-	void		send_text(gcs_severity severity, const char *str) {}
-
 	/// Send a text message with a PSTR()
 	///
 	/// @param	severity	A value describing the importance of the message.
 	/// @param	str			The text to be sent.
 	///
-	void		send_text(gcs_severity severity, const prog_char_t *str) {}
+	void		send_text_P(gcs_severity severity, const prog_char_t *str) {}
 
     // send streams which match frequency range
     void data_stream_send(void);
@@ -86,7 +77,7 @@ public:
 
 protected:
 	/// The stream we are communicating over
-	FastSerial			*_port;
+    AP_HAL::UARTDriver *      _port;
 };
 
 //
@@ -105,10 +96,9 @@ 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_P(gcs_severity severity, const prog_char_t *str);
-	void	send_text(gcs_severity severity, const prog_char_t *str);
     void    data_stream_send(void);
 	void    queued_param_send();
 	void    queued_waypoint_send();
@@ -132,6 +122,10 @@ public:
     // see if we should send a stream now. Called at 50Hz
     bool stream_trigger(enum streams stream_num);
 
+	// this costs us 51 bytes per instance, but means that low priority
+	// messages don't block the CPU
+    mavlink_statustext_t pending_status;
+
 private:
 	void 	handleMessage(mavlink_message_t * msg);
 

APMrover2/GCS_Mavlink.pde

@@ -3,10 +3,6 @@
 // use this to prevent recursion during sensor init
 static bool in_mavlink_delay;
 
-// this costs us 51 bytes, but means that low priority
-// messages don't block the CPU
-static mavlink_statustext_t pending_status;
-
 // true when we have received at least 1 MAVLink packet
 static bool mavlink_active;
 
@@ -192,8 +188,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
 
 static void NOINLINE send_meminfo(mavlink_channel_t chan)
 {
+#if CONFIG_HAL_BOARD != HAL_BOARD_AVR_SITL
     extern unsigned __brkval;
     mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
+#endif
 }
 
 static void NOINLINE send_location(mavlink_channel_t chan)
@@ -308,18 +306,34 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
 
 static void NOINLINE send_radio_out(mavlink_channel_t chan)
 {
-        mavlink_msg_servo_output_raw_send(
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
-            chan,
+    mavlink_msg_servo_output_raw_send(
-            micros(),
+        chan,
-            0, // port
+        micros(),
-            APM_RC.OutputCh_current(0),
+        0,     // port
-            APM_RC.OutputCh_current(1),
+        hal.rcout->read(0),
-            APM_RC.OutputCh_current(2),
+        hal.rcout->read(1),
-            APM_RC.OutputCh_current(3),
+        hal.rcout->read(2),
-            APM_RC.OutputCh_current(4),
+        hal.rcout->read(3),
-            APM_RC.OutputCh_current(5),
+        hal.rcout->read(4),
-            APM_RC.OutputCh_current(6),
+        hal.rcout->read(5),
-            APM_RC.OutputCh_current(7));
+        hal.rcout->read(6),
+        hal.rcout->read(7));
+#else
+    extern RC_Channel* rc_ch[8];
+    mavlink_msg_servo_output_raw_send(
+        chan,
+        micros(),
+        0,     // port
+        rc_ch[0]->radio_out,
+        rc_ch[1]->radio_out,
+        rc_ch[2]->radio_out,
+        rc_ch[3]->radio_out,
+        rc_ch[4]->radio_out,
+        rc_ch[5]->radio_out,
+        rc_ch[6]->radio_out,
+        rc_ch[7]->radio_out);
+#endif
 }
 
 static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
@@ -390,7 +404,7 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan)
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
 
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 // report simulator state
 static void NOINLINE send_simstate(mavlink_channel_t chan)
 {
@@ -398,15 +412,13 @@ static void NOINLINE send_simstate(mavlink_channel_t chan)
 }
 #endif
 
-#ifndef DESKTOP_BUILD
 static void NOINLINE send_hwstatus(mavlink_channel_t chan)
 {
     mavlink_msg_hwstatus_send(
         chan,
         board_voltage(),
-        I2c.lockup_count());
+        hal.i2c->lockup_count());
 }
-#endif
 
 static void NOINLINE send_gps_status(mavlink_channel_t chan)
 {
@@ -429,10 +441,11 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 
 static void NOINLINE send_statustext(mavlink_channel_t chan)
 {
+    mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
     mavlink_msg_statustext_send(
         chan,
-        pending_status.severity,
+        s->severity,
-        pending_status.text);
+        s->text);
 }
 
 // are we still delaying telemetry to try to avoid Xbee bricking?
@@ -581,17 +594,15 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         break;
 
     case MSG_SIMSTATE:
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
         CHECK_PAYLOAD_SIZE(SIMSTATE);
         send_simstate(chan);
 #endif
         break;
 
     case MSG_HWSTATUS:
-#ifndef DESKTOP_BUILD
         CHECK_PAYLOAD_SIZE(HWSTATUS);
         send_hwstatus(chan);
-#endif
         break;
 
     case MSG_RETRY_DEFERRED:
@@ -668,8 +679,9 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
 
     if (severity == SEVERITY_LOW) {
         // send via the deferred queuing system
-        pending_status.severity = (uint8_t)severity;
+        mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
-        strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
+        s->severity = (uint8_t)severity;
+        strncpy((char *)s->text, str, sizeof(s->text));
         mavlink_send_message(chan, MSG_STATUSTEXT, 0);
     } else {
         // send immediately
@@ -699,17 +711,17 @@ 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{
         mavlink_comm_1_port = port;
         chan = MAVLINK_COMM_1;
     }
-	_queued_parameter = NULL;
+    _queued_parameter = NULL;
 }
 
 void
@@ -881,13 +893,7 @@ GCS_MAVLINK::send_message(enum ap_message id)
 }
 
 void
-GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
+GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
-{
-    mavlink_send_text(chan,severity,str);
-}
-
-void
-GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
 {
     mavlink_statustext_t m;
     uint8_t i;
@@ -987,7 +993,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             uint8_t result;
 
             // do command
-            send_text(SEVERITY_LOW,PSTR("command received: "));
+            send_text_P(SEVERITY_LOW,PSTR("command received: "));
 
             switch(packet.command) {
 
@@ -1274,7 +1280,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         if (packet.start_index > g.command_total ||
             packet.end_index > g.command_total ||
             packet.end_index < packet.start_index) {
-            send_text(SEVERITY_LOW,PSTR("flight plan update rejected"));
+            send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));
             break;
         }
 
@@ -1457,7 +1463,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 						msg->compid,
 						result);
 
-					send_text(SEVERITY_LOW,PSTR("flight plan received"));
+					send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
 					waypoint_receiving = false;
 					// XXX ignores waypoint radius for individual waypoints, can
 					// only set WP_RADIUS parameter
@@ -1545,30 +1551,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         } // end case
 
     case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
-        {
+    {
-            // allow override of RC channel values for HIL
+        // allow override of RC channel values for HIL
-            // or for complete GCS control of switch position
+        // or for complete GCS control of switch position
-            // and RC PWM values.
+        // and RC PWM values.
-			if(msg->sysid != g.sysid_my_gcs) break;		// Only accept control from our gcs
+        if(msg->sysid != g.sysid_my_gcs) break;                         // Only accept control from our gcs
-            mavlink_rc_channels_override_t packet;
+        mavlink_rc_channels_override_t packet;
-            int16_t v[8];
+        int16_t v[8];
-            mavlink_msg_rc_channels_override_decode(msg, &packet);
+        mavlink_msg_rc_channels_override_decode(msg, &packet);
 
-			if (mavlink_check_target(packet.target_system,packet.target_component))
+        if (mavlink_check_target(packet.target_system,packet.target_component))
-				break;
-
-            v[0] = packet.chan1_raw;
-            v[1] = packet.chan2_raw;
-            v[2] = packet.chan3_raw;
-            v[3] = packet.chan4_raw;
-            v[4] = packet.chan5_raw;
-            v[5] = packet.chan6_raw;
-            v[6] = packet.chan7_raw;
-            v[7] = packet.chan8_raw;
-            rc_override_active = APM_RC.setHIL(v);
-			rc_override_fs_timer = millis();
             break;
-        }
+
+        v[0] = packet.chan1_raw;
+        v[1] = packet.chan2_raw;
+        v[2] = packet.chan3_raw;
+        v[3] = packet.chan4_raw;
+        v[4] = packet.chan5_raw;
+        v[5] = packet.chan6_raw;
+        v[6] = packet.chan7_raw;
+        v[7] = packet.chan8_raw;
+
+        hal.rcin->set_overrides(v, 8);
+
+        rc_override_fs_timer = millis();
+        break;
+    }
 
     case MAVLINK_MSG_ID_HEARTBEAT:
         {
@@ -1811,42 +1819,36 @@ GCS_MAVLINK::queued_waypoint_send()
 }
 
 /*
- a delay() callback that processes MAVLink packets. We set this as the
+ *  a delay() callback that processes MAVLink packets. We set this as the
- callback in long running library initialisation routines to allow
+ *  callback in long running library initialisation routines to allow
- MAVLink to process packets while waiting for the initialisation to
+ *  MAVLink to process packets while waiting for the initialisation to
- complete
+ *  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;
-    static uint32_t last_1hz, last_50hz;
+    if (!gcs0.initialised) return;
-
-    if (in_mavlink_delay) {
-        // this should never happen, but let's not tempt fate by
-        // letting the stack grow too much
-        delay(t);
-        return;
-    }
 
     in_mavlink_delay = true;
 
-    tstart = millis();
+    uint32_t tnow = millis();
-    do {
+    if (tnow - last_1hz > 1000) {
-        uint32_t tnow = millis();
+        last_1hz = tnow;
-        if (tnow - last_1hz > 1000) {
+        gcs_send_message(MSG_HEARTBEAT);
-            last_1hz = tnow;
+        gcs_send_message(MSG_EXTENDED_STATUS1);
-            gcs_send_message(MSG_HEARTBEAT);
+    }
-            gcs_send_message(MSG_EXTENDED_STATUS1);
+    if (tnow - last_50hz > 20) {
-        }
+        last_50hz = tnow;
-        if (tnow - last_50hz > 20) {
+        gcs_update();
-            last_50hz = tnow;
+        gcs_data_stream_send();
-            gcs_update();
+    }
-        }
+    if (tnow - last_5s > 5000) {
-        delay(1);
+        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;
 }
@@ -1886,31 +1888,32 @@ static void gcs_update(void)
 
 static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
 {
-    gcs0.send_text(severity, str);
+    gcs0.send_text_P(severity, str);
     if (gcs3.initialised) {
-        gcs3.send_text(severity, str);
+        gcs3.send_text_P(severity, str);
     }
 }
 
 /*
-  send a low priority formatted message to the GCS
+ *  send a low priority formatted message to the GCS
-  only one fits in the queue, so if you send more than one before the
+ *  only one fits in the queue, so if you send more than one before the
-  last one gets into the serial buffer then the old one will be lost
+ *  last one gets into the serial buffer then the old one will be lost
  */
-static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
+void gcs_send_text_fmt(const prog_char_t *fmt, ...)
 {
     char fmtstr[40];
-    va_list ap;
+    va_list arg_list;
     uint8_t i;
     for (i=0; i<sizeof(fmtstr)-1; i++) {
         fmtstr[i] = pgm_read_byte((const prog_char *)(fmt++));
         if (fmtstr[i] == 0) break;
     }
     fmtstr[i] = 0;
-    pending_status.severity = (uint8_t)SEVERITY_LOW;
+    gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
-    va_start(ap, fmt);
+    va_start(arg_list, fmt);
-    vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
+    vsnprintf((char *)gcs0.pending_status.text, sizeof(gcs0.pending_status.text), fmtstr, arg_list);
-    va_end(ap);
+    va_end(arg_list);
+    gcs3.pending_status = gcs0.pending_status;
     mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
     if (gcs3.initialised) {
         mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);

APMrover2/Log.pde

@@ -108,7 +108,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 	int16_t dump_log;
 	int16_t dump_log_start;
 	int16_t dump_log_end;
-	byte last_log_num;
+	uint8_t last_log_num;
 
 	// check that the requested log number can be read
 	dump_log = argv[1].i;
@@ -153,7 +153,7 @@ void erase_callback(unsigned long t) {
 static void do_erase_logs(void)
 {
 	cliSerial->printf_P(PSTR("\nErasing log...\n"));
-    DataFlash.EraseAll(erase_callback);
+    DataFlash.EraseAll();
 	cliSerial->printf_P(PSTR("\nLog erased.\n"));
 }
 
@@ -257,7 +257,7 @@ static void Log_Write_Performance()
 
 // 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);
@@ -271,7 +271,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp)
 	DataFlash.WriteByte(END_BYTE);
 }
 
-static void Log_Write_Startup(byte type)
+static void Log_Write_Startup(uint8_t type)
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
@@ -330,7 +330,7 @@ static void Log_Write_Nav_Tuning()
 }
 
 // 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);
@@ -341,7 +341,7 @@ static void Log_Write_Mode(byte mode)
 
 // 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)
+                            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);
@@ -468,7 +468,7 @@ 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: "));
@@ -489,7 +489,7 @@ static void Log_Read_Cmd()
 
 static void Log_Read_Startup()
 {
-	byte logbyte = DataFlash.ReadByte();
+	uint8_t logbyte = DataFlash.ReadByte();
 
 	if (logbyte == TYPE_AIRSTART_MSG)
 		cliSerial->printf_P(PSTR("AIR START - "));
@@ -524,7 +524,7 @@ static void Log_Read_Mode()
 static void Log_Read_GPS()
 {
     int32_t l[7];
-    byte b[2];
+    uint8_t b[2];
     int16_t j,k,m;
     l[0] = DataFlash.ReadLong();
     b[0] = DataFlash.ReadByte();
@@ -595,8 +595,8 @@ static void Log_Read(int16_t start_page, int16_t end_page)
 // Read the DataFlash log memory : Packet Parser
 static int Log_Read_Process(int16_t start_page, int16_t end_page)
 {
-	byte data;
+	uint8_t data;
-	byte log_step = 0;
+	uint8_t log_step = 0;
 	int page = start_page;
 	int packet_count = 0;
 
@@ -679,13 +679,13 @@ static int 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() {}
 static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
 static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {}

APMrover2/Makefile

@@ -18,15 +18,9 @@ hilnocli:
 heli:
 	make -f Makefile EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
 
-apm2:
-	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
-
 apm2beta:
 	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
 
-sitl:
-	make -f ../libraries/Desktop/Makefile.desktop
-
 etags:
 	cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)
 

APMrover2/Parameters.h

@@ -50,6 +50,9 @@ public:
         k_param_reset_switch_chan,
         k_param_manual_level,
         k_param_ins,                        // libraries/AP_InertialSensor variables        
+        k_param_rssi_pin,
+        k_param_battery_volt_pin,
+        k_param_battery_curr_pin,
 
 
 	// 110: Telemetry control
@@ -312,7 +315,11 @@ public:
         AP_Float    volt_div_ratio;
         AP_Float    curr_amp_per_volt;
         AP_Float    input_voltage;
-	AP_Int16    pack_capacity;		// Battery pack capacity less reserve
+		AP_Int16    pack_capacity;		// Battery pack capacity less reserve
+    	AP_Int8 rssi_pin;
+    	AP_Int8 battery_volt_pin;
+    	AP_Int8 battery_curr_pin;
+
 #if HIL_MODE != HIL_MODE_ATTITUDE
 #if CONFIG_SONAR == ENABLED     
         AP_Int8	    sonar_enabled;

APMrover2/Parameters.pde

@@ -201,6 +201,28 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(curr_amp_per_volt,      "AMP_PER_VOLT",     CURR_AMP_PER_VOLT),
 	GSCALAR(input_voltage,          "INPUT_VOLTS",      INPUT_VOLTAGE),
 	GSCALAR(pack_capacity,          "BATT_CAPACITY",    HIGH_DISCHARGE),
+
+    // @Param: BATT_VOLT_PIN
+    // @DisplayName: Battery Voltage sensing pin
+    // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13.
+    // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13
+    // @User: Standard
+    GSCALAR(battery_volt_pin,    "BATT_VOLT_PIN",    1),
+
+    // @Param: BATT_CURR_PIN
+    // @DisplayName: Battery Current sensing pin
+    // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13.
+    // @Values: -1:Disabled, 1:A1, 2:A2, 12:A12
+    // @User: Standard
+    GSCALAR(battery_curr_pin,    "BATT_CURR_PIN",    2),
+
+    // @Param: RSSI_PIN
+    // @DisplayName: Receiver RSSI sensing pin
+    // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
+    // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13
+    // @User: Standard
+    GSCALAR(rssi_pin,            "RSSI_PIN",         -1),
+
 #if HIL_MODE != HIL_MODE_ATTITUDE
 #if CONFIG_SONAR == ENABLED     
 	// @Param: SONAR_ENABLE
@@ -250,7 +272,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(ins,                            "INS_", AP_InertialSensor),
 #endif
 
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
     // @Group: SIM_
     // @Path: ../libraries/SITL/SITL.cpp
     GOBJECT(sitl, "SIM_", SITL),

APMrover2/commands.pde

@@ -27,7 +27,7 @@ static void init_commands()
 static struct Location get_cmd_with_index(int i)
 {
 	struct Location temp;
-	long mem;
+	uint16_t mem;
 
 	// Find out proper location in memory by using the start_byte position + the index
 	// --------------------------------------------------------------------------------
@@ -37,22 +37,22 @@ static struct Location get_cmd_with_index(int i)
 	}else{
 		// read WP position
 		mem = (WP_START_BYTE) + (i * WP_SIZE);
-		temp.id = eeprom_read_byte((uint8_t*)mem);
+		temp.id = hal.storage->read_byte(mem);
 
 		mem++;
-		temp.options = eeprom_read_byte((uint8_t*)mem);
+		temp.options = hal.storage->read_byte(mem);
 
 		mem++;
-		temp.p1 = eeprom_read_byte((uint8_t*)mem);
+		temp.p1 = hal.storage->read_byte(mem);
 
 		mem++;
-		temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
+		temp.alt = (long)hal.storage->read_dword(mem);
 
 		mem += 4;
-		temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
+		temp.lat = (long)hal.storage->read_dword(mem);
 
 		mem += 4;
-		temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
+		temp.lng = (long)hal.storage->read_dword(mem);
 	}
 
 	// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
@@ -68,7 +68,7 @@ static struct Location get_cmd_with_index(int i)
 static void set_cmd_with_index(struct Location temp, int i)
 {
 	i = constrain(i, 0, g.command_total.get());
-	intptr_t mem = WP_START_BYTE + (i * WP_SIZE);
+	uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
 
 	// Set altitude options bitmask
 	// XXX What is this trying to do?
@@ -78,22 +78,22 @@ static void set_cmd_with_index(struct Location temp, int i)
 		temp.options = 0;
 	}
 
-	eeprom_write_byte((uint8_t *)	mem, temp.id);
+	hal.storage->write_byte(mem, temp.id);
 
-        mem++;
+    mem++;
-	eeprom_write_byte((uint8_t *)	mem, temp.options);
+	hal.storage->write_byte(mem, temp.options);
 
 	mem++;
-	eeprom_write_byte((uint8_t *)	mem, temp.p1);
+	hal.storage->write_byte(mem, temp.p1);
 
 	mem++;
-	eeprom_write_dword((uint32_t *)	mem, temp.alt);
+	hal.storage->write_dword(mem, temp.alt);
 
 	mem += 4;
-	eeprom_write_dword((uint32_t *)	mem, temp.lat);
+	hal.storage->write_dword(mem, temp.lat);
 
 	mem += 4;
-	eeprom_write_dword((uint32_t *)	mem, temp.lng);
+	hal.storage->write_dword(mem, temp.lng);
 }
 
 /*

APMrover2/commands_logic.pde

@@ -363,7 +363,8 @@ static void do_set_home()
 
 static void do_set_servo()
 {
-	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()

APMrover2/commands_process.pde

@@ -50,7 +50,7 @@ static void process_next_command()
 	// and loads conditional or immediate commands if applicable
 
 	struct Location temp;
-	byte old_index = 0;
+	uint8_t old_index = 0;
 
 	// these are Navigation/Must commands
 	// ---------------------------------

APMrover2/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__
+

APMrover2/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);
+}
+

APMrover2/config.h

@@ -42,19 +42,11 @@
 //////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////
 
-//////////////////////////////////////////////////////////////////////////////
-// APM HARDWARE
-//
-
-#ifndef CONFIG_APM_HARDWARE
-# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // APM2 HARDWARE DEFAULTS
 //
 
-#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
 # define CONFIG_INS_TYPE   CONFIG_INS_MPU6000
 # define CONFIG_PUSHBUTTON DISABLED
 # define CONFIG_RELAY      DISABLED
@@ -66,7 +58,7 @@
 //////////////////////////////////////////////////////////////////////////////
 // LED and IO Pins
 //
-#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
 # define A_LED_PIN        37
 # define B_LED_PIN        36
 # define C_LED_PIN        35
@@ -78,7 +70,7 @@
 # define CONFIG_RELAY     ENABLED
 # define BATTERY_PIN_1	  0
 # define CURRENT_PIN_1	  1
-#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
 # define A_LED_PIN        27
 # define B_LED_PIN        26
 # define C_LED_PIN        25
@@ -90,9 +82,21 @@
 # define USB_MUX_PIN 23
 # define BATTERY_PIN_1	  1
 # define CURRENT_PIN_1	  2
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+# define A_LED_PIN        27
+# define B_LED_PIN        26
+# define C_LED_PIN        25
+# define LED_ON           LOW
+# define LED_OFF          HIGH
+# define SLIDE_SWITCH_PIN (-1)
+# define PUSHBUTTON_PIN   (-1)
+# define CLI_SLIDER_ENABLED DISABLED
+# define USB_MUX_PIN -1
+# define BATTERY_PIN_1	  1
+# define CURRENT_PIN_1	  2
 #endif
 
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #define CONFIG_SONAR DISABLED
 #endif
 
@@ -139,7 +143,7 @@
 # endif
 #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
 # ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN
-#  define CONFIG_SONAR_SOURCE_ANALOG_PIN A0
+#  define CONFIG_SONAR_SOURCE_ANALOG_PIN 0
 # endif
 #else
 # warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar

APMrover2/control_modes.pde

@@ -3,7 +3,7 @@
 static void read_control_switch()
 {
 	
-	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.
@@ -17,7 +17,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)) {
 
 		set_mode(flight_modes[switchPosition]);
 
@@ -32,8 +32,8 @@ static void read_control_switch()
 
 }
 
-static byte readSwitch(void){
+static uint8_t readSwitch(void){
-	uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
+    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;

APMrover2/defines.h

@@ -215,9 +215,6 @@ enum gcs_severity {
 #define CONFIG_INS_OILPAN 1
 #define CONFIG_INS_MPU6000 2
 
-#define APM_HARDWARE_APM1  1
-#define APM_HARDWARE_APM2 2
-
 #define AP_BARO_BMP085   1
 #define AP_BARO_MS5611   2
 

APMrover2/events.pde

@@ -32,7 +32,7 @@ static void failsafe_long_on_event(int 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
+    hal.rcin->clear_overrides();
 	failsafe = fstype;
 	switch(control_mode)
 	{
@@ -94,9 +94,9 @@ static void update_events(void)	// Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_
 
 		if (event_id >= CH_5 && event_id <= CH_8) {
 			if(event_repeat%2) {
-				APM_RC.OutputCh(event_id, event_value); // send to Servos
+				hal.rcout->write(event_id, event_value); // send to Servos
 			} else {
-				APM_RC.OutputCh(event_id, event_undo_value);
+				hal.rcout->write(event_id, event_undo_value);
 			}
 		}
 

APMrover2/failsafe.pde

@@ -38,9 +38,14 @@ 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();
-        for (uint8_t ch=0; ch<8; ch++) {
+        uint8_t start_ch = 0;
-            APM_RC.OutputCh(ch, APM_RC.InputCh(ch));
+        if (demoing_servos) {
+            start_ch = 1;
         }
+        for (uint8_t ch=start_ch; ch<4; ch++) {
+            hal.rcout->write(ch, hal.rcin->read(ch));
+        }
+        RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
     }
 }

APMrover2/planner.pde

@@ -1,53 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-// These are function definitions so the Menu can be constructed before the functions
-// are defined below. Order matters to the compiler.
-static int8_t	planner_gcs(uint8_t argc, 		const Menu::arg *argv);
-
-// Creates a constant array of structs representing menu options
-// and stores them in Flash memory, not RAM.
-// User enters the string in the console to call the functions on the right.
-// See class Menu in AP_Common for implementation details
-static const struct Menu::command planner_menu_commands[] PROGMEM = {
-	{"gcs",		planner_gcs},
-};
-
-// A Macro to create the Menu
-MENU(planner_menu, "planner", planner_menu_commands);
-
-static int8_t
-planner_mode(uint8_t argc, const Menu::arg *argv)
-{
-	cliSerial->printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
-	planner_menu.run();
-    return 0;
-}
-static int8_t
-planner_gcs(uint8_t argc, const Menu::arg *argv)
-{
-  gcs0.init(&Serial);
-
-#if USB_MUX_PIN < 0
-  // we don't have gcs3 if we have the USB mux setup
-  gcs3.init(&Serial3);
-#endif
-
-  int loopcount = 0;
-  while (1) {
-    if (millis()-fast_loopTimer > 19) {
-      fast_loopTimer      = millis();
-      
-      gcs_update();
-      
-      read_radio();
-      
-      gcs_data_stream_send();
-      if ((loopcount % 16) == 0) { // 3 hz
-          gcs_send_message(MSG_HEARTBEAT);
-      }
-      loopcount++;
-    }
-  }
-  return 0;
-}
-

APMrover2/radio.pde

@@ -2,7 +2,7 @@
 
 //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
 
 
 static void init_rc_in()
@@ -30,52 +30,50 @@ static void init_rc_in()
 
 static void init_rc_out()
 {
-  APM_RC.Init( &isr_registry );		// APM Radio initialization
+    hal.rcout->enable_ch(CH_1);
-
+    hal.rcout->enable_ch(CH_2);
-  APM_RC.enable_out(CH_1);
+    hal.rcout->enable_ch(CH_3);
-  APM_RC.enable_out(CH_2);
+    hal.rcout->enable_ch(CH_4);
-  APM_RC.enable_out(CH_3);
+    hal.rcout->enable_ch(CH_5);
-  APM_RC.enable_out(CH_4);
+    hal.rcout->enable_ch(CH_6);
-  APM_RC.enable_out(CH_5);
+    hal.rcout->enable_ch(CH_7);
-  APM_RC.enable_out(CH_6);
+    hal.rcout->enable_ch(CH_8);
-  APM_RC.enable_out(CH_7);
-  APM_RC.enable_out(CH_8);
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
-	APM_RC.OutputCh(CH_1, 	g.channel_roll.radio_trim);					// Initialization of servo outputs
+	hal.rcout->write(CH_1, 	g.channel_roll.radio_trim);					// Initialization of servo outputs
-	APM_RC.OutputCh(CH_2, 	g.channel_pitch.radio_trim);
+	hal.rcout->write(CH_2, 	g.channel_pitch.radio_trim);
-	APM_RC.OutputCh(CH_3, 	g.channel_throttle.radio_trim);
+	hal.rcout->write(CH_3, 	g.channel_throttle.radio_trim);
-	APM_RC.OutputCh(CH_4, 	g.channel_rudder.radio_trim);
+	hal.rcout->write(CH_4, 	g.channel_rudder.radio_trim);
 
-	APM_RC.OutputCh(CH_5, 	g.rc_5.radio_trim);
+	hal.rcout->write(CH_5, 	g.rc_5.radio_trim);
-	APM_RC.OutputCh(CH_6, 	g.rc_6.radio_trim);
+	hal.rcout->write(CH_6, 	g.rc_6.radio_trim);
-	APM_RC.OutputCh(CH_7,   g.rc_7.radio_trim);
+	hal.rcout->write(CH_7,   g.rc_7.radio_trim);
-    APM_RC.OutputCh(CH_8,   g.rc_8.radio_trim);
+    hal.rcout->write(CH_8,   g.rc_8.radio_trim);
 #else
-	APM_RC.OutputCh(CH_1, 	1500);					// Initialization of servo outputs
+	hal.rcout->write(CH_1, 	1500);					// Initialization of servo outputs
-	APM_RC.OutputCh(CH_2, 	1500);
+	hal.rcout->write(CH_2, 	1500);
-	APM_RC.OutputCh(CH_3, 	1000);
+	hal.rcout->write(CH_3, 	1000);
-	APM_RC.OutputCh(CH_4, 	1500);
+	hal.rcout->write(CH_4, 	1500);
 
-	APM_RC.OutputCh(CH_5, 	1500);
+	hal.rcout->write(CH_5, 	1500);
-	APM_RC.OutputCh(CH_6, 	1500);
+	hal.rcout->write(CH_6, 	1500);
-	APM_RC.OutputCh(CH_7,   1500);
+	hal.rcout->write(CH_7,   1500);
-    APM_RC.OutputCh(CH_8,   2000);
+    hal.rcout->write(CH_8,   2000);
 #endif
 
 }
 
 static void read_radio()
 {
-    g.channel_roll.set_pwm(APM_RC.InputCh(CH_ROLL));
+    g.channel_roll.set_pwm(hal.rcin->read(CH_ROLL));
-    g.channel_pitch.set_pwm(APM_RC.InputCh(CH_PITCH));
+    g.channel_pitch.set_pwm(hal.rcin->read(CH_PITCH));
 
-	g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
+	g.channel_throttle.set_pwm(hal.rcout->read(CH_3));
-	g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
+	g.channel_rudder.set_pwm(hal.rcout->read(CH_4));
-  	g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
+  	g.rc_5.set_pwm(hal.rcout->read(CH_5));
- 	g.rc_6.set_pwm(APM_RC.InputCh(CH_6));        
+ 	g.rc_6.set_pwm(hal.rcout->read(CH_6));        
-	g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
+	g.rc_7.set_pwm(hal.rcout->read(CH_7));
-	g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
+	g.rc_8.set_pwm(hal.rcout->read(CH_8));
 
 	control_failsafe(g.channel_throttle.radio_in);
 

APMrover2/sensors.pde

@@ -28,15 +28,17 @@ static void read_battery(void)
 		return;
 	}
 	
-	if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
+    if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
-        static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1);
+        // this copes with changing the pin at runtime
-		battery_voltage1 = BATTERY_VOLTAGE(bat_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) {
+        // 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());
+        current_total1   += current_amps1 * (float)delta_ms_medium_loop * 0.0002778;                                    // .0002778 is 1/3600 (conversion to hours)
     }
-	if(g.battery_monitoring == 4) {
-        static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1);
-		current_amps1	 = CURRENT_AMPS(current_pin.read_average());
-		current_total1	 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778;				// .0002778 is 1/3600 (conversion to hours)
-	}
 	
 	#if BATTERY_EVENT == ENABLED
 		if(battery_voltage1 < LOW_VOLTAGE)	low_battery_event();

APMrover2/setup.pde

@@ -169,7 +169,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();
@@ -191,7 +193,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();
@@ -294,34 +296,14 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
   handle full accelerometer calibration via user dialog
  */
 #if !defined( __AVR_ATmega1280__ )
-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);
+             flash_leds);
-    if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
+    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);
@@ -538,7 +520,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);
@@ -597,10 +579,10 @@ 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++) {
+	for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) {
-		eeprom_write_byte((uint8_t *) i, b);
+		hal.storage->write_byte(i, b);
 	}
 	cliSerial->printf_P(PSTR("done\n"));
 }
@@ -614,18 +596,4 @@ static void print_enabled(bool b)
 	cliSerial->printf_P(PSTR("abled\n"));
 }
 
-static void
-print_accel_offsets_and_scaling(void)
-{
-    Vector3f accel_offsets = ins.get_accel_offsets();
-    Vector3f accel_scale = ins.get_accel_scale();
-    cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
-                    (float)accel_offsets.x,                           // Pitch
-                    (float)accel_offsets.y,                           // Roll
-                    (float)accel_offsets.z,                           // YAW
-                    (float)accel_scale.x,                             // Pitch
-                    (float)accel_scale.y,                             // Roll
-                    (float)accel_scale.z);                            // YAW
-}
-
 #endif // CLI_ENABLED

APMrover2/system.pde

@@ -14,7 +14,6 @@ static int8_t	process_logs(uint8_t argc, const Menu::arg *argv);	// in Log.pde
 #endif
 static int8_t	setup_mode(uint8_t argc, const Menu::arg *argv);	// in setup.pde
 static int8_t	test_mode(uint8_t argc, const Menu::arg *argv);		// in test.cpp
-static int8_t	planner_mode(uint8_t argc, const Menu::arg *argv);	// in planner.pde
 
 // This is the help function
 // PSTR is an AVR macro to read strings from flash memory
@@ -40,18 +39,17 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
 #endif
 	{"setup",		setup_mode},
 	{"test",		test_mode},
-	{"help",		main_menu_help},
+	{"help",		main_menu_help}
-	{"planner",		planner_mode}
 };
 
 // Create the top-level menu object.
 MENU(main_menu, THISFIRMWARE, main_menu_commands);
 
 // 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);
@@ -93,7 +91,7 @@ static void init_ardupilot()
 	// XXX This could be optimised to reduce the buffer sizes in the cases
 	// where they are not otherwise required.
 	//
-	cliSerial->begin(SERIAL0_BAUD, 128, 128);
+    hal.uartA->begin(SERIAL0_BAUD, 128, 128);
 
 	// GPS serial port.
 	//
@@ -105,37 +103,12 @@ static void init_ardupilot()
 	// on the message set configured.
 	//
     // standard gps running
-    Serial1.begin(115200, 128, 16);
+    hal.uartB->begin(115200, 128, 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.
 	//
@@ -147,18 +120,22 @@ 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
-        cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+        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, 128);
+    hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
-	gcs3.init(&Serial3);
+	gcs3.init(hal.uartC);
 #endif
 
 	mavlink_system.sysid = g.sysid_this_mav;
@@ -182,7 +159,7 @@ static void init_ardupilot()
 #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
 
 #if LITE == DISABLED
@@ -239,15 +216,14 @@ static void init_ardupilot()
 	// Do GPS init
 	g_gps = &g_gps_driver;
     // GPS initialisation
-	g_gps->init(GPS::GPS_ENGINE_AUTOMOTIVE);
+	g_gps->init(hal.uartB, GPS::GPS_ENGINE_AUTOMOTIVE);
 
 	//mavlink_system.sysid = MAV_SYSTEM_ID;				// Using g.sysid_this_mav
 	mavlink_system.compid = 1;	//MAV_COMP_ID_IMU;   // We do not check for comp id
 	mavlink_system.type = MAV_TYPE_GROUND_ROVER;
 
-	rc_override_active = APM_RC.setHIL(rc_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
 
@@ -261,15 +237,14 @@ static void init_ardupilot()
 	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
 
     /*
       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);
-
 
 	// If the switch is in 'menu' mode, run the main menu.
 	//
@@ -293,7 +268,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
 
@@ -353,7 +328,7 @@ static void startup_ground(void)
 	gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
 }
 
-static void set_mode(byte mode)
+static void set_mode(uint8_t mode)
 {       
 
 	if(control_mode == mode){
@@ -449,12 +424,12 @@ 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 (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);
 	}
 	ahrs.set_fly_forward(true);
     ahrs.reset();
@@ -538,9 +513,9 @@ static void check_usb_mux(void)
     // the user has switched to/from the telemetry port
     usb_connected = usb_check;
     if (usb_connected) {
-        cliSerial->begin(SERIAL0_BAUD, 128, 128);
+        hal.uartA->begin(SERIAL0_BAUD, 128, 128);
     } else {
-        cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+        hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
     }
 }
 #endif
@@ -556,17 +531,13 @@ void flash_leds(bool on)
     digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
 }
 
-#ifndef DESKTOP_BUILD
 /*
  * Read Vcc vs 1.1v internal reference
  */
 uint16_t board_voltage(void)
 {
-    static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC);
+    return vcc_pin->read_latest();
-    return vcc.read_vcc();
 }
-#endif
-
 
 static void
 print_flight_mode(uint8_t mode)

APMrover2/test.pde

@@ -20,9 +20,6 @@ static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_sonar(uint8_t argc, 	const Menu::arg *argv);
 #endif
 static int8_t	test_mag(uint8_t argc, 			const Menu::arg *argv);
-static int8_t	test_xbee(uint8_t argc, 		const Menu::arg *argv);
-static int8_t	test_eedump(uint8_t argc, 		const Menu::arg *argv);
-static int8_t	test_rawgps(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_modeswitch(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_logging(uint8_t argc, 		const Menu::arg *argv);
 
@@ -38,8 +35,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"battery",	test_battery},
 	{"relay",			test_relay},
 	{"waypoints",		test_wp},
-	{"xbee",			test_xbee},
-	{"eedump",			test_eedump},
 	{"modeswitch",		test_modeswitch},
 
 	// Tests below here are for hardware sensors only present
@@ -49,7 +44,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"adc", 		test_adc},
 #endif
 	{"gps",			test_gps},
-	{"rawgps",		test_rawgps},
 	{"ins",			test_ins},
 #if CONFIG_SONAR == ENABLED
 	{"sonartest",	test_sonar},
@@ -82,21 +76,6 @@ static void print_hit_enter()
 	cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
 }
 
-static int8_t
-test_eedump(uint8_t argc, const Menu::arg *argv)
-{
-	intptr_t i, j;
-
-	// hexdump the EEPROM
-	for (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)));
-		cliSerial->println();
-	}
-	return(0);
-}
-
 static int8_t
 test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 {
@@ -137,12 +116,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("CH:");
             for(int i = 0; i < 8; i++){
-                cliSerial->print(APM_RC.InputCh(i));	// Print channel values
+                cliSerial->print(hal.rcout->read(i));	// Print channel values
                 cliSerial->print(",");
-                APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
+                hal.rcout->write(i, hal.rcout->read(i)); // Copy input to Servos
             }
             cliSerial->println();
         }
@@ -198,7 +177,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(int i = 0; i < 50; i++){
 		delay(20);
@@ -317,7 +296,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 	cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
 	cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_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);
 	}
@@ -326,7 +305,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,
@@ -338,25 +317,6 @@ test_wp_print(struct Location *cmd, byte wp_index)
 		cmd->lng);
 }
 
-static int8_t
-test_xbee(uint8_t argc, const Menu::arg *argv)
-{
-	print_hit_enter();
-	delay(1000);
-	cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
-
-	while(1){
-
-	    if (Serial3.available())
-   			Serial3.write(Serial3.read());
-
-		if(cliSerial->available() > 0){
-			return (0);
-		}
-	}
-}
-
-
 static int8_t
 test_modeswitch(uint8_t argc, const Menu::arg *argv)
 {
@@ -369,7 +329,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"),  switchPosition);
 			oldSwitchPosition = switchPosition;
@@ -415,7 +375,7 @@ 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);
@@ -467,7 +427,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();
@@ -530,7 +490,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();
 
 	int counter = 0;
@@ -597,31 +557,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 //-------------------------------------------------------------------------------------------
 // real sensors that have not been simulated yet go here
 
-#if HIL_MODE == HIL_MODE_DISABLED
-
-static int8_t
-test_rawgps(uint8_t argc, const Menu::arg *argv)
-{
-  print_hit_enter();
-	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());
-			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());
-			digitalWrite(C_LED_PIN, LED_OFF);
-		}
-		if(cliSerial->available() > 0){
-			return (0);
-		}
-  }
-}
-
 #if CONFIG_SONAR == ENABLED
 static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
@@ -634,7 +569,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
 	while(1){
 	  delay(20);
 	  if(g.sonar_enabled){
-		sonar_dist = sonar.read();
+		sonar_dist = sonar->read();
 	  }
     	  cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
 
@@ -645,6 +580,5 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
   return (0);
 }
 #endif // SONAR == ENABLED
-#endif // HIL_MODE == HIL_MODE_DISABLED
 
 #endif // CLI_ENABLED