From 27dab27565a56eb84a20101567a1762264d6075f Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 15 Oct 2016 10:42:45 +1100 Subject: [PATCH] SITL: Add parameters for sensor position offsets in body frame --- libraries/SITL/SITL.cpp | 4 ++++ libraries/SITL/SITL.h | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 31016aa573..95ef43c334 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -83,6 +83,10 @@ const AP_Param::GroupInfo SITL::var_info[] = { AP_GROUPINFO("PIN_MASK", 50, SITL, pin_mask, 0), AP_GROUPINFO("ADSB_TX", 51, SITL, adsb_tx, 0), AP_GROUPINFO("SPEEDUP", 52, SITL, speedup, -1), + AP_GROUPINFO("IMU_POS", 53, SITL, imu_pos_offset, 0), + AP_GROUPINFO("GPS_POS", 54, SITL, gps_pos_offset, 0), + AP_GROUPINFO("SONAR_POS", 55, SITL, rngfnd_pos_offset, 0), + AP_GROUPINFO("FLOW_POS", 56, SITL, optflow_pos_offset, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index afb53cde04..21ad82d745 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -127,6 +127,12 @@ public: AP_Vector3f mag_anomaly_ned; // NED anomaly vector at ground level (mGauss) AP_Float mag_anomaly_hgt; // height above ground where anomally strength has decayed to 1/8 of the ground level value (m) + // Body frame sensor position offsets + AP_Vector3f imu_pos_offset; // XYZ position of the IMU accelerometer relative to the body frame origin (m) + AP_Vector3f gps_pos_offset; // XYZ position of the GPS antenna phase centre relative to the body frame origin (m) + AP_Vector3f rngfnd_pos_offset; // XYZ position of the range finder zero range datum relative to the body frame origin (m) + AP_Vector3f optflow_pos_offset; // XYZ position of the optical flow sensor focal point relative to the body frame origin (m) + void simstate_send(mavlink_channel_t chan); void Log_Write_SIMSTATE(DataFlash_Class *dataflash);