From 24e1af1c82b5b97eca9d60586f656fca579b871d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Dec 2012 23:26:39 +1100 Subject: [PATCH] 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 | 6 +- ArduPlane/GCS_Mavlink.pde | 116 +++++++++----------------------------- ArduPlane/Parameters.pde | 4 +- ArduPlane/config.h | 5 ++ ArduPlane/test.pde | 1 - 5 files changed, 34 insertions(+), 98 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 60c37e642f..8af8b113db 100644 --- a/ArduPlane/ArduPlane.pde +++ b/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 diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index c07a66a238..00d9afa7f4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/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,13 +1813,23 @@ 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); + // approximate a barometer + float y; + const float Temp = 312; + + y = (packet.alt - 584000.0) / 29271.267; + y /= (Temp / 10.0) + 273.15; + y = 1.0/exp(y); + y *= 95446.0; + + barometer.setHIL(Temp, y); #else @@ -1848,64 +1842,6 @@ mission_failed: 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); - - // 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); - - // set pressure hil sensor - // TODO: check scaling - float temp = 70; - barometer.setHIL(temp,packet.press_diff1 + 101325); - break; - } -#endif // HIL_MODE #if CAMERA == ENABLED case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index aecb297b64..d960c6986f 100644 --- a/ArduPlane/Parameters.pde +++ b/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 + GOBJECT(ins, "INS_", AP_InertialSensor), // @Group: AHRS_ // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 8055181385..54e1b1bb38 100644 --- a/ArduPlane/config.h +++ b/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 ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 92ab01d336..f952e38030 100644 --- a/ArduPlane/test.pde +++ b/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},