mirror of https://github.com/ArduPilot/ardupilot
152 lines
3.9 KiB
C++
152 lines
3.9 KiB
C++
/*
|
|
SITL handling
|
|
|
|
This emulates the ADS7844 ADC
|
|
|
|
Andrew Tridgell November 2011
|
|
*/
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#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
|
|
*/
|
|
static uint16_t airspeed_sensor(float airspeed)
|
|
{
|
|
const float airspeed_ratio = 1.9936;
|
|
const float airspeed_offset = 2820;
|
|
float airspeed_pressure, airspeed_raw;
|
|
|
|
airspeed_pressure = sqr(airspeed) / airspeed_ratio;
|
|
airspeed_raw = airspeed_pressure + airspeed_offset;
|
|
return airspeed_raw;
|
|
}
|
|
|
|
|
|
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
|
|
|
|
Note that this uses roll, pitch and yaw only as inputs. The
|
|
simulator rollrates are instantaneous, whereas we need to use
|
|
average rates to cope with slow update rates.
|
|
|
|
inputs are in degrees
|
|
|
|
phi - roll
|
|
theta - pitch
|
|
psi - true heading
|
|
alpha - angle of attack
|
|
beta - side slip
|
|
phidot - roll rate
|
|
thetadot - pitch rate
|
|
psidot - yaw rate
|
|
v_north - north velocity in local/body frame
|
|
v_east - east velocity in local/body frame
|
|
v_down - down velocity in local/body frame
|
|
A_X_pilot - X accel in body frame
|
|
A_Y_pilot - Y accel in body frame
|
|
A_Z_pilot - Z accel in body frame
|
|
|
|
Note: doubles on high prec. stuff are preserved until the last moment
|
|
|
|
*/
|
|
void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth
|
|
double rollRate, double pitchRate,double yawRate, // Local to plane
|
|
double xAccel, double yAccel, double zAccel, // Local to plane
|
|
float airspeed)
|
|
{
|
|
static const uint8_t sensor_map[6] = { 1, 2, 0, 4, 5, 6 };
|
|
static const float _sensor_signs[6] = { 1, -1, -1, 1, -1, -1 };
|
|
const float accel_offset = 2041;
|
|
const float gyro_offset = 1658;
|
|
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 p, q, r;
|
|
extern bool sitl_motors_on;
|
|
|
|
SITL::convert_body_frame(roll, pitch,
|
|
rollRate, pitchRate, yawRate,
|
|
&p, &q, &r);
|
|
|
|
// minimum noise levels are 2 bits
|
|
float accel_noise = _accel_scale*2;
|
|
float gyro_noise = _gyro_gain_y*2;
|
|
if (sitl_motors_on) {
|
|
// add extra noise when the motors are on
|
|
accel_noise += sitl.accel_noise;
|
|
gyro_noise += ToRad(sitl.gyro_noise);
|
|
}
|
|
xAccel += accel_noise * rand_float();
|
|
yAccel += accel_noise * rand_float();
|
|
zAccel += accel_noise * rand_float();
|
|
|
|
p += gyro_noise * rand_float();
|
|
q += gyro_noise * rand_float();
|
|
r += 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;
|
|
adc[1] = (q / (_gyro_gain_y * _sensor_signs[1])) + gyro_offset;
|
|
adc[2] = (r / (_gyro_gain_z * _sensor_signs[2])) + gyro_offset;
|
|
|
|
adc[3] = (xAccel / (_accel_scale * _sensor_signs[3])) + accel_offset;
|
|
adc[4] = (yAccel / (_accel_scale * _sensor_signs[4])) + accel_offset;
|
|
adc[5] = (zAccel / (_accel_scale * _sensor_signs[5])) + accel_offset;
|
|
|
|
/* tell the UDR2 register emulation what values to give to the driver */
|
|
for (uint8_t i=0; i<6; i++) {
|
|
UDR2.set(sensor_map[i], adc[i]);
|
|
}
|
|
// set the airspeed sensor input
|
|
UDR2.set(7, airspeed_sensor(airspeed));
|
|
|
|
/* FIX: rubbish value for temperature for now */
|
|
UDR2.set(3, 2000);
|
|
|
|
runInterrupt(6);
|
|
}
|
|
|
|
|
|
/*
|
|
setup ADC emulation
|
|
*/
|
|
void sitl_setup_adc(void)
|
|
{
|
|
// mark it always ready. This is the register
|
|
// the ADC driver uses to tell if there is new data pending
|
|
UCSR2A = (1 << RXC2);
|
|
}
|