mirror of https://github.com/ArduPilot/ardupilot
SITL: added SITL class for controlling simulation
this holds mavlink settable parameters for controlling sensor noise levels
This commit is contained in:
Andrew Tridgell
parent
37494dda2b
commit
117bae9585
|
|
@ -25,30 +25,13 @@
|
|||
#include <wiring.h>
|
||||
#include <AP_PeriodicProcess.h>
|
||||
#include <AP_TimerProcess.h>
|
||||
#include <SITL.h>
|
||||
#include <avr/interrupt.h>
|
||||
#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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,12 +11,15 @@
|
|||
#include <sys/types.h>
|
||||
#include <math.h>
|
||||
#include <AP_ADC.h>
|
||||
#include <SITL.h>
|
||||
#include <avr/interrupt.h>
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -10,54 +10,6 @@
|
|||
#include <math.h>
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -11,9 +11,12 @@
|
|||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <AP_Baro.h> // ArduPilot Mega BMP085 Library
|
||||
#include <SITL.h>
|
||||
#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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
#include <errno.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Compass.h>
|
||||
#include <SITL.h>
|
||||
#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);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
#include <SITL.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef __SITL_H__
|
||||
#define __SITL_H__
|
||||
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
|
||||
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__
|
||||
Loading…
Reference in New Issue