From ae6a94a933277ce52e0ee208f04d48c229a283b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Mar 2012 15:22:11 +1100 Subject: [PATCH] SITL: added sitl_simstate_send() used to report simulator state in logs --- libraries/Desktop/support/desktop.h | 2 ++ libraries/Desktop/support/sitl.cpp | 23 +++++++++++++++++++++++ libraries/Desktop/support/sitl_adc.cpp | 17 +++-------------- libraries/Desktop/support/util.cpp | 21 +++++++++++++++++++++ libraries/Desktop/support/util.h | 5 +++++ 5 files changed, 54 insertions(+), 14 deletions(-) diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h index 710980599e..8ff6756ddf 100644 --- a/libraries/Desktop/support/desktop.h +++ b/libraries/Desktop/support/desktop.h @@ -30,4 +30,6 @@ void sitl_update_adc(float roll, float pitch, float yaw, void sitl_setup_adc(void); void sitl_update_barometer(float altitude); +void sitl_simstate_send(uint8_t chan); + #endif diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 5e6ecfdbae..cb67d441e6 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "sitl_adc.h" #include "sitl_rc.h" #include "desktop.h" @@ -302,3 +303,25 @@ void sitl_setup(void) sitl_update_compass(0, 0, 0, 0); sitl_update_gps(0, 0, 0, 0, 0, false); } + + +/* report SITL state via MAVLink */ +void sitl_simstate_send(uint8_t chan) +{ + double p, q, r; + + // we want the gyro values to be directly comparable to the + // raw_imu message, which is in body frame + convert_body_frame(sim_state.rollDeg, sim_state.pitchDeg, + sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate, + &p, &q, &r); + + mavlink_msg_simstate_send((mavlink_channel_t)chan, + ToRad(sim_state.rollDeg), + ToRad(sim_state.pitchDeg), + ToRad(sim_state.yawDeg), + sim_state.xAccel, + sim_state.yAccel, + sim_state.zAccel, + p, q, r); +} diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index 63e44c2111..05fdaab4d1 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -68,27 +68,16 @@ void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth static const float _sensor_signs[6] = { 1, -1, -1, 1, -1, -1 }; const float accel_offset = 2041; const float gyro_offset = 1658; -#define ToRad(x) (x*0.01745329252) // *pi/180 const float _gyro_gain_x = ToRad(0.4); const float _gyro_gain_y = ToRad(0.41); const float _gyro_gain_z = ToRad(0.41); const float _accel_scale = 9.80665 / 423.8; double adc[7]; - double phi, theta, phiDot, thetaDot, psiDot; double p, q, r; - /* convert the angular velocities from earth frame to - body frame. Thanks to James Goppert for the formula - */ - phi = ToRad(roll); - theta = ToRad(pitch); - phiDot = ToRad(rollRate); - thetaDot = ToRad(pitchRate); - psiDot = ToRad(yawRate); - - p = phiDot - psiDot*sin(theta); - q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta); - r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot; + convert_body_frame(roll, pitch, + rollRate, pitchRate, yawRate, + &p, &q, &r); /* work out the ADC channel values */ adc[0] = (p / (_gyro_gain_x * _sensor_signs[0])) + gyro_offset; diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp index 6232f00aac..7656d36bf1 100644 --- a/libraries/Desktop/support/util.cpp +++ b/libraries/Desktop/support/util.cpp @@ -14,6 +14,7 @@ #include #include #include "desktop.h" +#include "util.h" void set_nonblocking(int fd) @@ -37,3 +38,23 @@ double normalise180(double v) { return normalise(v, -180, 180); } + +/* convert the angular velocities from earth frame to + body frame. Thanks to James Goppert for the formula +*/ +void convert_body_frame(double rollDeg, double pitchDeg, + double rollRate, double pitchRate, double yawRate, + double *p, double *q, double *r) +{ + double phi, theta, phiDot, thetaDot, psiDot; + + phi = ToRad(rollDeg); + theta = ToRad(pitchDeg); + phiDot = ToRad(rollRate); + thetaDot = ToRad(pitchRate); + psiDot = ToRad(yawRate); + + *p = phiDot - psiDot*sin(theta); + *q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta); + *r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot; +} diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h index 3cfc729292..7d55f827c6 100644 --- a/libraries/Desktop/support/util.h +++ b/libraries/Desktop/support/util.h @@ -2,8 +2,13 @@ #define ft2m(x) ((x) * 0.3048) #define kt2mps(x) ((x) * 0.514444444) #define sqr(x) ((x)*(x)) +#define ToRad(x) (x*0.01745329252) // *pi/180 void set_nonblocking(int fd); double normalise(double v, double min, double max); double normalise180(double v); void runInterrupt(uint8_t inum); + +void convert_body_frame(double rollDeg, double pitchDeg, + double rollRate, double pitchRate, double yawRate, + double *p, double *q, double *r);