From 1cb96e14a9b57ab2d801c4ef0effd94a376e69fa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Jun 2012 15:10:35 +1000 Subject: [PATCH] SITL: enable the SIM_* parameters in ArduCopter and ArduPlane --- ArduCopter/ArduCopter.pde | 2 ++ ArduCopter/GCS_Mavlink.pde | 18 +----------------- ArduCopter/Parameters.h | 4 ++-- ArduCopter/Parameters.pde | 4 ++++ ArduPlane/ArduPlane.pde | 2 ++ ArduPlane/GCS_Mavlink.pde | 18 +----------------- ArduPlane/Parameters.h | 4 +++- ArduPlane/Parameters.pde | 7 ++++++- 8 files changed, 21 insertions(+), 38 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ac7a20704f..fee9b67c37 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -186,6 +186,8 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #ifdef DESKTOP_BUILD AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; +#include + SITL sitl; #else #if CONFIG_BARO == AP_BARO_BMP085 diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index f78ff63d40..074ad5f876 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -280,26 +280,10 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan) #endif // HIL_MODE != HIL_MODE_ATTITUDE #ifdef DESKTOP_BUILD -void mavlink_simstate_send(uint8_t chan, - float roll, - float pitch, - float yaw, - float xAcc, - float yAcc, - float zAcc, - float p, - float q, - float r) -{ - mavlink_msg_simstate_send((mavlink_channel_t)chan, - roll, pitch, yaw, xAcc, yAcc, zAcc, p, q, r); -} - // report simulator state static void NOINLINE send_simstate(mavlink_channel_t chan) { - extern void sitl_simstate_send(uint8_t chan); - sitl_simstate_send((uint8_t)chan); + sitl.simstate_send(chan); } #endif diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 296fb0bd90..bb3a7429b2 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -51,13 +51,14 @@ public: k_param_format_version = 0, k_param_software_type, + // simulation + k_param_sitl = 10, // Misc // k_param_log_bitmask = 20, k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change - #if FRAME_CONFIG == HELI_FRAME // // 80: Heli // @@ -65,7 +66,6 @@ public: k_param_heli_servo_2, k_param_heli_servo_3, k_param_heli_servo_4, - #endif // // 90: Motors diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index bba9b1519d..f064545ed9 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -227,6 +227,10 @@ static const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), +#ifdef DESKTOP_BUILD + GOBJECT(sitl, "SIM_", SITL), +#endif + #if FRAME_CONFIG == HELI_FRAME // @Group: H_ // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 202fb611f5..656d0b03a0 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -150,6 +150,8 @@ static AP_ADC_ADS7844 adc; #ifdef DESKTOP_BUILD AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; +#include +SITL sitl; #else #if CONFIG_BARO == AP_BARO_BMP085 diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index abad8dbf44..07c35ec0a4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -501,26 +501,10 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan) #endif // HIL_MODE != HIL_MODE_ATTITUDE #ifdef DESKTOP_BUILD -void mavlink_simstate_send(uint8_t chan, - float roll, - float pitch, - float yaw, - float xAcc, - float yAcc, - float zAcc, - float p, - float q, - float r) -{ - mavlink_msg_simstate_send((mavlink_channel_t)chan, - roll, pitch, yaw, xAcc, yAcc, zAcc, p, q, r); -} - // report simulator state static void NOINLINE send_simstate(mavlink_channel_t chan) { - extern void sitl_simstate_send(uint8_t chan); - sitl_simstate_send((uint8_t)chan); + sitl.simstate_send(chan); } #endif diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6f5e4bcfa8..c0ecd278f2 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -70,7 +70,6 @@ public: k_param_reset_switch_chan, k_param_manual_level, - // 110: Telemetry control // k_param_gcs0 = 110, // stream rates for port0 @@ -185,6 +184,9 @@ public: k_param_fence_minalt, k_param_fence_maxalt, + // simulator control + k_param_sitl = 230, + // // 240: PID Controllers // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 4fbea8dee0..b9cc38c6bb 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -326,7 +326,12 @@ static const AP_Param::Info var_info[] PROGMEM = { // @Group: AHRS_ // @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp - GOBJECT(ahrs, "AHRS_", AP_AHRS) + GOBJECT(ahrs, "AHRS_", AP_AHRS), + +#ifdef DESKTOP_BUILD + // @Group: SITL + GOBJECT(sitl, "SIM_", SITL), +#endif };