diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 8ff7fc6808..ea774b83e9 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -25,30 +25,13 @@ #include #include #include +#include #include #include "sitl_adc.h" #include "sitl_rc.h" #include "desktop.h" #include "util.h" -/* - the sitl_fdm packet is received by the SITL build from the flight - simulator. This is used to feed the internal sensor emulation - */ -struct sitl_fdm { - // little-endian packet format - double latitude, longitude; // degrees - double altitude; // MSL - double heading; // degrees - double speedN, speedE; // m/s - double xAccel, yAccel, zAccel; // m/s/s in body frame - double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame - double rollDeg, pitchDeg, yawDeg; // euler angles, degrees - double airspeed; // m/s - uint32_t magic; // 0x4c56414e -}; - - #define SIMIN_PORT 5501 #define RCOUT_PORT 5502 @@ -61,8 +44,8 @@ struct ADC_UDR2 UDR2; struct RC_ICR4 ICR4; extern AP_TimerProcess timer_scheduler; extern Arduino_Mega_ISR_Registry isr_registry; +extern SITL sitl; -static struct sitl_fdm sim_state; static uint32_t update_count; @@ -133,7 +116,7 @@ static void sitl_fdm_input(void) return; } - sim_state = d.fg_pkt; + sitl.state = d.fg_pkt; update_count++; count++; @@ -276,15 +259,15 @@ static void timer_handler(int signum) } last_update_count = update_count; - sitl_update_gps(sim_state.latitude, sim_state.longitude, - sim_state.altitude, - sim_state.speedN, sim_state.speedE, true); - sitl_update_adc(sim_state.rollDeg, sim_state.pitchDeg, sim_state.yawDeg, - sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate, - sim_state.xAccel, sim_state.yAccel, sim_state.zAccel, - sim_state.airspeed); - sitl_update_barometer(sim_state.altitude); - sitl_update_compass(sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading); + sitl_update_gps(sitl.state.latitude, sitl.state.longitude, + sitl.state.altitude, + sitl.state.speedN, sitl.state.speedE, !sitl.gps_disable); + sitl_update_adc(sitl.state.rollDeg, sitl.state.pitchDeg, sitl.state.yawDeg, + sitl.state.rollRate, sitl.state.pitchRate, sitl.state.yawRate, + sitl.state.xAccel, sitl.state.yAccel, sitl.state.zAccel, + sitl.state.airspeed); + sitl_update_barometer(sitl.state.altitude); + sitl_update_compass(sitl.state.rollDeg, sitl.state.pitchDeg, sitl.state.heading); sei(); running = false; } @@ -338,40 +321,3 @@ void sitl_setup(void) } -/* report SITL state via MAVLink */ -void sitl_simstate_send(uint8_t chan) -{ - double p, q, r; - float yaw; - extern 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); - - // 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); - - // convert to same conventions as DCM - yaw = sim_state.yawDeg; - if (yaw > 180) { - yaw -= 360; - } - - mavlink_simstate_send(chan, - ToRad(sim_state.rollDeg), - ToRad(sim_state.pitchDeg), - ToRad(yaw), - 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 b3339d9143..a017d03667 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -11,12 +11,15 @@ #include #include #include +#include #include #include "wiring.h" #include "sitl_adc.h" #include "desktop.h" #include "util.h" +extern SITL sitl; + /* convert airspeed in m/s to an airspeed sensor value */ @@ -32,6 +35,20 @@ static uint16_t airspeed_sensor(float airspeed) } +static float gyro_drift(void) +{ + if (sitl.drift_speed == 0.0) { + return 0; + } + double period = sitl.drift_time * 2; + double minutes = fmod(micros() / 60.0e6, period); + if (minutes < period/2) { + return minutes * ToRad(sitl.drift_speed); + } + return (period - minutes) * ToRad(sitl.drift_speed); + +} + /* setup the ADC channels with new input @@ -74,10 +91,33 @@ void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth const float _accel_scale = 9.80665 / 423.8; double adc[7]; double p, q, r; + extern float sitl_motor_speed[4]; + bool motors_on = false; - convert_body_frame(roll, pitch, - rollRate, pitchRate, yawRate, - &p, &q, &r); + SITL::convert_body_frame(roll, pitch, + rollRate, pitchRate, yawRate, + &p, &q, &r); + + for (uint8_t i=0; i<4; i++) { + if (sitl_motor_speed[i] > 0.0) { + motors_on = true; + } + } + + if (motors_on) { + // only add accel/gyro noise when motors are on + xAccel += sitl.accel_noise * rand_float(); + yAccel += sitl.accel_noise * rand_float(); + zAccel += sitl.accel_noise * rand_float(); + + p += ToRad(sitl.gyro_noise) * rand_float(); + q += ToRad(sitl.gyro_noise) * rand_float(); + r += ToRad(sitl.gyro_noise) * rand_float(); + } + + p += gyro_drift(); + q += gyro_drift(); + r += gyro_drift(); /* work out the ADC channel values */ adc[0] = (p / (_gyro_gain_x * _sensor_signs[0])) + gyro_offset; diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h index 348d433ff9..4087c34968 100644 --- a/libraries/Desktop/support/sitl_adc.h +++ b/libraries/Desktop/support/sitl_adc.h @@ -10,54 +10,6 @@ #include #include "util.h" -static const float vibration_level = 0.2; -static const float drift_speed = 0.2; // degrees/second/minute -static const float drift_time = 5; // time to reverse drift direction (minutes) -// order zgyro, xgyro, ygyro, temp, xacc, yacc, zacc, aspd -static const float noise_scale[8] = { 150, 150, 150, 0, 400, 400, 400, 0 }; -static const float noise_offset[8]= { 0, 0, 0, 0, 0, 0, 0, 0 }; -static const float drift_rate[8] = { 1.0, 1.0, 1.0, 0, 0, 0, 0, 0 }; -static const float base_noise = 2; - -static inline float gyro_drift(uint8_t chan) -{ - if (drift_rate[chan] * drift_speed == 0.0) { - return 0; - } - extern long unsigned int micros(void); - double period = drift_rate[chan] * drift_time * 2; - double minutes = fmod(micros() / 60.0e6, period); - if (minutes < period/2) { - return minutes * drift_speed / 0.4; - } - return (period - minutes) * drift_speed / 0.4; - -} - -static inline float noise_generator(uint8_t chan) -{ - extern float sitl_motor_speed[4]; - uint8_t i; - float noise = 0; - uint8_t noise_count=0; - extern long unsigned int micros(void); - for (i=0; i<4; i++) { - if (sitl_motor_speed[i] > 0.0) { - float n = rand_float() * noise_scale[chan] * vibration_level; - //double t = micros() / 1.0e6; - //float freq = (rand_float() + 1.0) * sitl_motor_speed[i]; - //noise += sin(fmod(t * freq * 2 * M_PI + i, - //2*M_PI)) * n; - noise += n + noise_offset[chan]; - noise_count++; - } - } - if (noise_count == 0) { - return gyro_drift(chan) + rand_float() * base_noise * vibration_level; - } - return gyro_drift(chan) + noise/noise_count; -} - // this implements the UDR2 register struct ADC_UDR2 { uint16_t value, next_value; @@ -94,7 +46,6 @@ struct ADC_UDR2 { } next_analog = channels[chan]; idx = 1; - next_analog += noise_generator(chan) + 0.5; if (next_analog > 0xFFF) next_analog = 0xFFF; if (next_analog < 0) next_analog = 0; next_value = ((unsigned)next_analog) << 3; diff --git a/libraries/Desktop/support/sitl_barometer.cpp b/libraries/Desktop/support/sitl_barometer.cpp index 91b081adce..2be39737e6 100644 --- a/libraries/Desktop/support/sitl_barometer.cpp +++ b/libraries/Desktop/support/sitl_barometer.cpp @@ -11,9 +11,12 @@ #include #include #include // ArduPilot Mega BMP085 Library +#include #include "desktop.h" #include "util.h" +extern SITL sitl; + /* setup the barometer with new input altitude is in meters @@ -30,7 +33,7 @@ void sitl_update_barometer(float altitude) y = 1.0/exp(y); y *= 95446.0; - Press = y; + Press = y + (rand_float() * sitl.baro_noise); barometer.setHIL(Temp, Press); } diff --git a/libraries/Desktop/support/sitl_compass.cpp b/libraries/Desktop/support/sitl_compass.cpp index 39912e8959..deb37c38ce 100644 --- a/libraries/Desktop/support/sitl_compass.cpp +++ b/libraries/Desktop/support/sitl_compass.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include "desktop.h" #include "util.h" @@ -30,7 +31,7 @@ // using an APM1 with 5883L compass #define MAG_FIELD_STRENGTH 818 -#define MAG_NOISE 5 +extern SITL sitl; /* given a magnetic heading, and roll, pitch, yaw values, @@ -55,7 +56,7 @@ static Vector3f heading_to_mag(float roll, float pitch, float yaw) // convert the earth frame magnetic vector to body frame, and // apply the offsets m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z); - return m + (rand_vec3f() * MAG_NOISE); + return m + (rand_vec3f() * sitl.mag_noise); } diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp index 6a956533e3..2a9a8a93f5 100644 --- a/libraries/Desktop/support/util.cpp +++ b/libraries/Desktop/support/util.cpp @@ -40,26 +40,6 @@ 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; -} - // generate a random Vector3f of size 1 Vector3f rand_vec3f(void) { diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h index 7778adda14..a4e54cce0e 100644 --- a/libraries/Desktop/support/util.h +++ b/libraries/Desktop/support/util.h @@ -9,10 +9,6 @@ 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); - // generate a random float between -1 and 1 #define rand_float() (((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp new file mode 100644 index 0000000000..47ab9c8d43 --- /dev/null +++ b/libraries/SITL/SITL.cpp @@ -0,0 +1,75 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + SITL.cpp - software in the loop state + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation; either version 2.1 + of the License, or (at your option) any later version. +*/ + +#include +#include +#include +#include + +// table of user settable parameters +const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { + AP_GROUPINFO("BARO_RND", 0, SITL, baro_noise), + AP_GROUPINFO("GYR_RND", 1, SITL, gyro_noise), + AP_GROUPINFO("ACC_RND", 2, SITL, accel_noise), + AP_GROUPINFO("MAG_RND", 3, SITL, mag_noise), + AP_GROUPINFO("GPS_DISABLE",4, SITL, gps_disable), + AP_GROUPINFO("DRIFT_SPEED",5, SITL, drift_speed), + AP_GROUPINFO("DRIFT_TIME", 6, SITL, drift_time), + AP_GROUPEND +}; + + + +/* report SITL state via MAVLink */ +void SITL::simstate_send(mavlink_channel_t chan) +{ + double p, q, r; + float yaw; + + // we want the gyro values to be directly comparable to the + // raw_imu message, which is in body frame + convert_body_frame(state.rollDeg, state.pitchDeg, + state.rollRate, state.pitchRate, state.yawRate, + &p, &q, &r); + + // convert to same conventions as DCM + yaw = state.yawDeg; + if (yaw > 180) { + yaw -= 360; + } + + mavlink_msg_simstate_send(chan, + ToRad(state.rollDeg), + ToRad(state.pitchDeg), + ToRad(yaw), + state.xAccel, + state.yAccel, + state.zAccel, + p, q, r); +} + +// convert a set of roll rates from earth frame to body frame +void SITL::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/SITL/SITL.h b/libraries/SITL/SITL.h new file mode 100644 index 0000000000..dfd159eb13 --- /dev/null +++ b/libraries/SITL/SITL.h @@ -0,0 +1,62 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __SITL_H__ +#define __SITL_H__ + +#include +#include +#include +#include + +struct sitl_fdm { + // this is the packet sent by the simulator + // to the APM executable to update the simulator state + // All values are little-endian + double latitude, longitude; // degrees + double altitude; // MSL + double heading; // degrees + double speedN, speedE; // m/s + double xAccel, yAccel, zAccel; // m/s/s in body frame + double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame + double rollDeg, pitchDeg, yawDeg; // euler angles, degrees + double airspeed; // m/s + uint32_t magic; // 0x4c56414e +}; + + +class SITL +{ +public: + SITL() { + baro_noise = 10; // Pascals + gyro_noise = 30; // degrees/s + accel_noise = 3; // m/s/s + mag_noise = 10; // mag units + aspd_noise = 2; // m/s + drift_speed = 0.2; // dps/min + drift_time = 5; // minutes + } + struct sitl_fdm state; + + static const struct AP_Param::GroupInfo var_info[]; + + // noise levels for simulated sensors + AP_Float baro_noise; // in Pascals + AP_Float gyro_noise; // in degrees/second + AP_Float accel_noise; // in m/s/s + AP_Float mag_noise; // in mag units (earth field is 818) + AP_Float aspd_noise; // in m/s + + AP_Float drift_speed; // degrees/second/minute + AP_Float drift_time; // period in minutes + AP_Int8 gps_disable; // disable simulated GPS + + void simstate_send(mavlink_channel_t chan); + + // convert a set of roll rates from earth frame to body frame + static void convert_body_frame(double rollDeg, double pitchDeg, + double rollRate, double pitchRate, double yawRate, + double *p, double *q, double *r); +}; + +#endif // __SITL_H__