From 3bac27b97f0952cb3b977aa26174777b467be58f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Dec 2012 11:13:33 +1100 Subject: [PATCH] Plane: enable attitude and raw logging in HIL modes --- ArduPlane/ArduPlane.pde | 19 ++++--------------- ArduPlane/GCS_Mavlink.pde | 14 +++----------- ArduPlane/Log.pde | 6 ------ ArduPlane/Makefile | 11 +++++++---- ArduPlane/commands.pde | 4 ++-- ArduPlane/config.h | 1 + ArduPlane/sensors.pde | 1 + 7 files changed, 18 insertions(+), 38 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 8af8b113db..7acf89fda0 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -240,15 +240,11 @@ AP_InertialSensor_Stub ins; AP_AHRS_DCM ahrs(&ins, g_gps); #elif HIL_MODE == HIL_MODE_ATTITUDE -AP_InertialSensor_Stub ins; -AP_AHRS_HIL ahrs(&ins, g_gps); -AP_GPS_HIL g_gps_driver(NULL); -AP_Compass_HIL compass; // never used AP_Baro_BMP085_HIL barometer; - #ifdef DESKTOP_BUILD - #include -SITL sitl; - #endif +AP_Compass_HIL compass; +AP_GPS_HIL g_gps_driver(NULL); +AP_InertialSensor_Stub ins; +AP_AHRS_HIL ahrs(&ins, g_gps); #else #error Unrecognised HIL_MODE setting. @@ -718,10 +714,7 @@ void loop() if (millis() - perf_mon_timer > 20000) { if (mainLoop_count != 0) { if (g.log_bitmask & MASK_LOG_PM) -#if HIL_MODE != HIL_MODE_ATTITUDE Log_Write_Performance(); -#endif - resetPerfData(); } } @@ -769,13 +762,11 @@ static void fast_loop() // uses the yaw from the DCM to give more accurate turns calc_bearing_error(); -# if HIL_MODE == HIL_MODE_DISABLED if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); if (g.log_bitmask & MASK_LOG_RAW) Log_Write_Raw(); -#endif // inertial navigation // ------------------ @@ -886,13 +877,11 @@ static void medium_loop() case 3: medium_loopCounter++; -#if HIL_MODE != HIL_MODE_ATTITUDE if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); if (g.log_bitmask & MASK_LOG_CTUN) Log_Write_Control_Tuning(); -#endif if (g.log_bitmask & MASK_LOG_NTUN) Log_Write_Nav_Tuning(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 00d9afa7f4..ebf8d7bfe8 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -379,7 +379,6 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) barometer.get_climb_rate()); } -#if HIL_MODE != HIL_MODE_ATTITUDE static void NOINLINE send_raw_imu1(mavlink_channel_t chan) { Vector3f accel = ins.get_accel(); @@ -445,8 +444,6 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan) ahrs.get_error_yaw()); } -#endif // HIL_MODE != HIL_MODE_ATTITUDE - #ifdef DESKTOP_BUILD // report simulator state static void NOINLINE send_simstate(mavlink_channel_t chan) @@ -587,7 +584,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, send_vfr_hud(chan); break; -#if HIL_MODE != HIL_MODE_ATTITUDE case MSG_RAW_IMU1: CHECK_PAYLOAD_SIZE(RAW_IMU); send_raw_imu1(chan); @@ -602,7 +598,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); send_raw_imu3(chan); break; -#endif // HIL_MODE != HIL_MODE_ATTITUDE case MSG_CURRENT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_CURRENT); @@ -640,10 +635,8 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, #endif case MSG_AHRS: -#if HIL_MODE != HIL_MODE_ATTITUDE CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(chan); -#endif break; case MSG_SIMSTATE: @@ -1803,7 +1796,6 @@ mission_failed: packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, vel*1.0e-2, cog*1.0e-2, 0, 10); - #if HIL_MODE == HIL_MODE_SENSORS // rad/sec Vector3f gyros; @@ -1831,9 +1823,9 @@ mission_failed: barometer.setHIL(Temp, y); - #else - - // set AHRS hil sensor + #if HIL_MODE == HIL_MODE_ATTITUDE + // set AHRS hil sensor. We don't do this in sensors mode, as + // in that case the attitude is computed via DCM ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, packet.pitchspeed,packet.yawspeed); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 1f378e7bfc..fc9c2ed36d 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -222,7 +222,6 @@ static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log } // Write a performance monitoring packet. Total length : 19 bytes - #if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Performance() { DataFlash.WriteByte(HEAD_BYTE1); @@ -243,7 +242,6 @@ static void Log_Write_Performance() DataFlash.WriteInt(pmTest1); DataFlash.WriteByte(END_BYTE); } - #endif // 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) @@ -282,7 +280,6 @@ static void Log_Write_Startup(byte type) // Write a control tuning packet. Total length : 22 bytes - #if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Control_Tuning() { Vector3f accel = ins.get_accel(); @@ -301,7 +298,6 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt((int)(accel.y * 10000)); DataFlash.WriteByte(END_BYTE); } - #endif // Write a navigation tuning packet. Total length : 18 bytes static void Log_Write_Nav_Tuning() @@ -350,7 +346,6 @@ static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t } // Write an raw accel/gyro data packet. Total length : 28 bytes - #if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Raw() { Vector3f gyro = ins.get_gyro(); @@ -370,7 +365,6 @@ static void Log_Write_Raw() DataFlash.WriteByte(END_BYTE); } - #endif static void Log_Write_Current() { diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 2954787023..704706cf31 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -7,16 +7,19 @@ nogps: make -f Makefile EXTRAFLAGS="-DGPS_PROTOCOL=GPS_PROTOCOL_NONE -DLOGGING_ENABLED=DISABLED" hil: - make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE" + +hil-apm2: + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" hilsensors: - make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS" hilsensors-apm2: - make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" hilnocli: - make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED" nocli: make -f Makefile EXTRAFLAGS="-DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index f2f17837bb..ab00ed674e 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -250,8 +250,8 @@ void init_home() while (!g_gps->new_data || !g_gps->fix) { g_gps->update(); #if HIL_MODE != HIL_MODE_DISABLED - // update hil gps so we have new_data - gcs_update(); + // update hil gps so we have new_data + gcs_update(); #endif } diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 54e1b1bb38..15b9b242da 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -190,6 +190,7 @@ #define CONFIG_ADC DISABLED #undef CONFIG_PITOT_SOURCE #define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN + #undef CONFIG_PITOT_SOURCE_ANALOG_PIN #define CONFIG_PITOT_SOURCE_ANALOG_PIN -1 #endif diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 29abdbd0fb..6b09753ff8 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -6,6 +6,7 @@ static LowPassFilterInt32 altitude_filter; static void init_barometer(void) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer")); barometer.calibrate(mavlink_delay); // filter at 100ms sampling, with 0.7Hz cutoff frequency