Plane: fixed sensors and attitude HIL

we now use the Stub version of the InertialSensor driver. In sensors
HIL we can now correctly drive the AHRS code.

ArduPlane/ArduPlane.pde

@@ -233,16 +233,14 @@ AP_AHRS_DCM ahrs(&ins, g_gps);
 
 #elif HIL_MODE == HIL_MODE_SENSORS
 // sensor emulators
-AP_ADC_HIL adc;
 AP_Baro_BMP085_HIL barometer;
 AP_Compass_HIL compass;
 AP_GPS_HIL              g_gps_driver(NULL);
-AP_InertialSensor_Oilpan ins( &adc );
+AP_InertialSensor_Stub ins;
 AP_AHRS_DCM  ahrs(&ins, g_gps);
 
 #elif HIL_MODE == HIL_MODE_ATTITUDE
-AP_ADC_HIL adc;
-AP_InertialSensor_Oilpan ins( &adc );
+AP_InertialSensor_Stub ins;
 AP_AHRS_HIL             ahrs(&ins, g_gps);
 AP_GPS_HIL              g_gps_driver(NULL);
 AP_Compass_HIL compass;          // never used

ArduPlane/GCS_Mavlink.pde

@@ -886,6 +886,17 @@ GCS_MAVLINK::data_stream_send(void)
     }
 
     if (in_mavlink_delay) {
+#if HIL_MODE != HIL_MODE_DISABLED
+        // in HIL we need to keep sending servo values to ensure
+        // the simulator doesn't pause, otherwise our sensor
+        // calibration could stall
+        if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+            send_message(MSG_SERVO_OUT);
+        }
+        if (stream_trigger(STREAM_RC_CHANNELS)) {
+            send_message(MSG_RADIO_OUT);
+        }
+#endif
         // don't send any other stream types while in the delay callback
         return;
     }
@@ -1779,33 +1790,6 @@ mission_failed:
     }
 
 #if HIL_MODE != HIL_MODE_DISABLED
-    // This is used both as a sensor and to pass the location
-    // in HIL_ATTITUDE mode.
-    case MAVLINK_MSG_ID_GPS_RAW_INT:
-    {
-        // decode
-        mavlink_gps_raw_int_t packet;
-        mavlink_msg_gps_raw_int_decode(msg, &packet);
-
-        // set gps hil sensor
-        g_gps->setHIL(packet.time_usec/1000,
-                      packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
-                      packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 10);
-        break;
-    }
-
-    //    Is this resolved? - MAVLink protocol change.....
-    case MAVLINK_MSG_ID_VFR_HUD:
-    {
-        // decode
-        mavlink_vfr_hud_t packet;
-        mavlink_msg_vfr_hud_decode(msg, &packet);
-
-        // set airspeed
-        airspeed.set_HIL(packet.airspeed);
-        break;
-    }
-
     case MAVLINK_MSG_ID_HIL_STATE:
     {
         mavlink_hil_state_t packet;
@@ -1829,80 +1813,32 @@ mission_failed:
 
         // m/s/s
         Vector3f accels;
-        accels.x = (float)packet.xacc / 1000.0;
-        accels.y = (float)packet.yacc / 1000.0;
-        accels.z = (float)packet.zacc / 1000.0;
+        accels.x = packet.xacc * (gravity/1000.0);
+        accels.y = packet.yacc * (gravity/1000.0);
+        accels.z = packet.zacc * (gravity/1000.0);
 
-        ins.set_gyro_offsets(gyros);
+        ins.set_gyro(gyros);
+        ins.set_accel(accels);
 
-        ins.set_accel_offsets(accels);
-
- #else
+        // approximate a barometer
+        float y;
+        const float Temp = 312;
 
-        // set AHRS hil sensor
-        ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
-                    packet.pitchspeed,packet.yawspeed);
+        y = (packet.alt - 584000.0) / 29271.267;
+        y /= (Temp / 10.0) + 273.15;
+        y = 1.0/exp(y);
+        y *= 95446.0;
 
- #endif
+        barometer.setHIL(Temp, y);
 
-        break;
-    }
-#endif // HIL_MODE
-#if HIL_MODE == HIL_MODE_ATTITUDE
-    case MAVLINK_MSG_ID_ATTITUDE:
-    {
-        // decode
-        mavlink_attitude_t packet;
-        mavlink_msg_attitude_decode(msg, &packet);
+ #else
 
         // set AHRS hil sensor
         ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
                     packet.pitchspeed,packet.yawspeed);
-        break;
-    }
-#endif
-#if HIL_MODE == HIL_MODE_SENSORS
-
-    case MAVLINK_MSG_ID_RAW_IMU:
-    {
-        // decode
-        mavlink_raw_imu_t packet;
-        mavlink_msg_raw_imu_decode(msg, &packet);
 
-        // set imu hil sensors
-        // TODO: check scaling for temp/absPress
-        //float temp = 70;
-        //float absPress = 1;
-
-        // rad/sec
-        Vector3f gyros;
-        gyros.x = (float)packet.xgyro / 1000.0;
-        gyros.y = (float)packet.ygyro / 1000.0;
-        gyros.z = (float)packet.zgyro / 1000.0;
-        // m/s/s
-        Vector3f accels;
-        accels.x = (float)packet.xacc / 1000.0;
-        accels.y = (float)packet.yacc / 1000.0;
-        accels.z = (float)packet.zacc / 1000.0;
-
-        ins.set_gyro_offsets(gyros);
-
-        ins.set_accel_offsets(accels);
-
-        compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
-        break;
-    }
-
-    case MAVLINK_MSG_ID_RAW_PRESSURE:
-    {
-        // decode
-        mavlink_raw_pressure_t packet;
-        mavlink_msg_raw_pressure_decode(msg, &packet);
+ #endif
 
-        // set pressure hil sensor
-        // TODO: check scaling
-        float temp = 70;
-        barometer.setHIL(temp,packet.press_diff1 + 101325);
         break;
     }
 #endif // HIL_MODE

ArduPlane/Parameters.pde

@@ -660,11 +660,9 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(gcs0,                                   "SR0_",     GCS_MAVLINK),
     GOBJECT(gcs3,                                   "SR3_",     GCS_MAVLINK),
 
-#if HIL_MODE == HIL_MODE_DISABLED
     // @Group: INS_
     // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
     GOBJECT(ins,                    "INS_", AP_InertialSensor),
-#endif
 
     // @Group: AHRS_
     // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp

ArduPlane/config.h

@@ -186,6 +186,11 @@
 #if HIL_MODE != HIL_MODE_DISABLED       // we are in HIL mode
  # undef GPS_PROTOCOL
  # define GPS_PROTOCOL GPS_PROTOCOL_NONE
+ #undef CONFIG_ADC
+ #define CONFIG_ADC DISABLED
+ #undef CONFIG_PITOT_SOURCE
+ #define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN
+ #define CONFIG_PITOT_SOURCE_ANALOG_PIN -1
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduPlane/test.pde

@@ -54,7 +54,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
     {"airpressure", test_pressure},
     {"compass",             test_mag},
 #elif HIL_MODE == HIL_MODE_SENSORS
-    {"adc",                 test_adc},
     {"gps",                 test_gps},
     {"ins",                 test_ins},
     {"compass",             test_mag},