Copter: fixed SITL for ArduCopter

ArduCopter/ArduCopter.pde

@@ -67,6 +67,7 @@
 #include <AP_HAL.h>
 #include <AP_HAL_AVR.h>
 #include <AP_HAL_AVR_SITL.h>
+#include <AP_HAL_Empty.h>
 
 // Application dependencies
 #include <GCS_MAVLink.h>        // MAVLink GCS definitions
@@ -97,6 +98,7 @@
 #include <AP_Declination.h>     // ArduPilot Mega Declination Helper Library
 #include <AP_Limits.h>
 #include <memcheck.h>
+#include <SITL.h>
 
 // AP_HAL to Arduino compatibility layer
 #include "compat.h"
@@ -123,15 +125,7 @@ AP_HAL::BetterStream* cliSerial;
 // AP_HAL instance
 ////////////////////////////////////////////////////////////////////////////////
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
-const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
-#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
-const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
-#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-const AP_HAL::HAL& hal = AP_HAL_AVR_SITL;
-#include <SITL.h>
-SITL sitl;
-#endif
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -192,14 +186,17 @@ AP_ADC_ADS7844 adc;
 
  #if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
 AP_InertialSensor_MPU6000 ins;
- #else
+#elif CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
 AP_InertialSensor_Oilpan ins(&adc);
+#elif CONFIG_IMU_TYPE == CONFIG_IMU_SITL
+AP_InertialSensor_Stub ins;
  #endif
 
  #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
  // When building for SITL we use the HIL barometer and compass drivers
 AP_Baro_BMP085_HIL barometer;
 AP_Compass_HIL compass;
+SITL sitl;
  #else
 // Otherwise, instantiate a real barometer and compass driver
   #if CONFIG_BARO == AP_BARO_BMP085
@@ -995,9 +992,6 @@ void loop()
             }
         }
     } else {
-#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-        usleep(1000);
-#endif
         if (timer - fast_loopTimer < 9) {
             // we have some spare cycles available
             // less than 10ms has passed. We have at least one millisecond

ArduCopter/GCS.h

@@ -61,20 +61,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
@@ -110,7 +102,7 @@ public:
     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);
+    void        send_text_P(gcs_severity severity, const prog_char_t *str);
     void        data_stream_send(void);
     void        queued_param_send();
     void        queued_waypoint_send();

ArduCopter/GCS_Mavlink.pde

@@ -194,8 +194,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)
@@ -252,7 +254,7 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan)
         ahrs.get_error_yaw());
 }
 
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 // report simulator state
 static void NOINLINE send_simstate(mavlink_channel_t chan)
 {
@@ -260,7 +262,6 @@ 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(
@@ -268,8 +269,6 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
         board_voltage(),
         hal.i2c->lockup_count());
 }
-#endif
-
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 {
@@ -639,17 +638,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:
@@ -955,13 +952,7 @@ GCS_MAVLINK::send_message(enum ap_message id)
 }
 
 void
-GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
-{
-    mavlink_send_text(chan,severity,str);
-}
-
-void
-GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
+GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
 {
     mavlink_statustext_t m;
     uint8_t i;
@@ -1062,7 +1053,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         uint8_t result = MAV_RESULT_UNSUPPORTED;
 
         // do command
-        send_text(SEVERITY_LOW,PSTR("command received: "));
+        send_text_P(SEVERITY_LOW,PSTR("command received: "));
 
         switch(packet.command) {
 
@@ -1642,7 +1633,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     msg->compid,
                     type);
 
-                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
@@ -1944,7 +1935,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         mavlink_fence_point_t packet;
         mavlink_msg_fence_point_decode(msg, &packet);
         if (packet.count != geofence_limit.fence_total()) {
-            send_text(SEVERITY_LOW,PSTR("bad fence point"));
+            send_text_P(SEVERITY_LOW,PSTR("bad fence point"));
         } else {
             Vector2l point;
             point.x = packet.lat*1.0e7;
@@ -1960,7 +1951,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         if (mavlink_check_target(packet.target_system, packet.target_component))
             break;
         if (packet.idx >= geofence_limit.fence_total()) {
-            send_text(SEVERITY_LOW,PSTR("bad fence point"));
+            send_text_P(SEVERITY_LOW,PSTR("bad fence point"));
         } else {
             Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx);
             mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(),
@@ -2111,19 +2102,11 @@ static void gcs_update(void)
     }
 }
 
-static void gcs_send_text(gcs_severity severity, const char *str)
-{
-    gcs0.send_text(severity, str);
-    if (gcs3.initialised) {
-        gcs3.send_text(severity, str);
-    }
-}
-
 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);
     }
 }
 

ArduCopter/Makefile

@@ -42,9 +42,6 @@ apm2hexa: apm2
 apm2beta: EXTRAFLAGS += "-DAPM2_BETA_HARDWARE "
 apm2beta: apm2
 
-sitl: HAL_BOARD = "HAL_BOARD_AVR_SITL"
-sitl: all
-
 sitl-octa: EXTRAFLAGS += "-DFRAME_CONFIG=OCTA_FRAME "
 sitl-octa: sitl
 

ArduCopter/Parameters.h

@@ -58,6 +58,9 @@ public:
         // simulation
         k_param_sitl = 10,
 
+        // barometer object (needed for SITL)
+        k_param_barometer,
+
         // Misc
         //
         k_param_log_bitmask = 20,

ArduCopter/Parameters.pde

@@ -577,10 +577,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(camera_mount2,           "MNT2_",       AP_Mount),
 #endif
 
-#ifdef DESKTOP_BUILD
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
     GOBJECT(sitl, "SIM_", SITL),
 #endif
 
+    GOBJECT(barometer, "GND_", AP_Baro),
+
     //@Group: LIM_
     //@Path: ../libraries/AP_Limits/AP_Limits.cpp,../libraries/AP_Limits/AP_Limit_GPSLock.cpp, ../libraries/AP_Limits/AP_Limit_Geofence.cpp, ../libraries/AP_Limits/AP_Limit_Altitude.cpp, ../libraries/AP_Limits/AP_Limit_Module.cpp
     GOBJECT(limits,                 "LIM_",    AP_Limits),

ArduCopter/config.h

@@ -64,6 +64,13 @@
  # else // APM2 Production Hardware (default)
   #  define CONFIG_BARO     AP_BARO_MS5611
  # endif
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ # define CONFIG_IMU_TYPE   CONFIG_IMU_SITL
+ # define CONFIG_PUSHBUTTON DISABLED
+ # define CONFIG_RELAY      DISABLED
+ # define MAG_ORIENTATION   AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
+ # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
+ # define MAGNETOMETER ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -157,6 +164,18 @@
  # define USB_MUX_PIN      23
  # define BATTERY_VOLT_PIN      1      // Battery voltage on A1
  # define BATTERY_CURR_PIN      2      // Battery current on A2
+#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_VOLT_PIN 1      // Battery voltage on A1
+ # define BATTERY_CURR_PIN 2      // Battery current on A2
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -179,7 +198,7 @@
  #define COPTER_LED_6 AN9       // Motor LED
  #define COPTER_LED_7 AN10      // Motor LED
  #define COPTER_LED_8 AN11      // Motor LED
-#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
  #define COPTER_LED_1 AN8       // Motor or Aux LED
  #define COPTER_LED_2 AN9       // Motor LED
  #define COPTER_LED_3 AN10      // Motor or GPS LED

ArduCopter/defines.h

@@ -410,8 +410,9 @@ enum gcs_severity {
 #define NOINLINE __attribute__((noinline))
 
 // IMU selection
-#define CONFIG_IMU_OILPAN 1
+#define CONFIG_IMU_OILPAN  1
 #define CONFIG_IMU_MPU6000 2
+#define CONFIG_IMU_SITL    3
 
 #define AP_BARO_BMP085    1
 #define AP_BARO_MS5611    2

ArduCopter/motors.pde

@@ -104,7 +104,7 @@ static void init_arm_motors()
     failsafe_disable();
 
     //cliSerial->printf("\nARM\n");
-#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
+#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
     gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
 #endif
 
@@ -170,7 +170,7 @@ static void init_arm_motors()
 
 static void init_disarm_motors()
 {
-#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
+#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
     gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
 #endif
 

ArduCopter/system.pde

@@ -625,7 +625,6 @@ void flash_leds(bool on)
     digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
 }
 
-#ifndef DESKTOP_BUILD
 /*
  * Read Vcc vs 1.1v internal reference
  */
@@ -633,7 +632,6 @@ uint16_t board_voltage(void)
 {
     return board_vcc_analog_source->read_latest();
 }
-#endif
 
 /*
   force a software reset of the APM