You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
130 lines
3.6 KiB
130 lines
3.6 KiB
/* |
|
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_DCM.h> |
|
#include <AP_ADC.h> |
|
#include "wiring.h" |
|
#include "sitl_adc.h" |
|
#include "desktop.h" |
|
#include "util.h" |
|
|
|
/* |
|
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; |
|
} |
|
|
|
|
|
/* |
|
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 |
|
*/ |
|
void sitl_update_adc(float roll, float pitch, float yaw, |
|
float rollRate, float pitchRate, float yawRate, |
|
float xAccel, float yAccel, float zAccel, |
|
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; |
|
#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; |
|
float adc[7]; |
|
float phi, theta, phiDot, thetaDot, psiDot; |
|
float 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; |
|
|
|
/* 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); |
|
|
|
#if 0 |
|
extern AP_DCM_HIL dcm; |
|
dcm.setHil(ToRad(roll), ToRad(pitch), ToRad(yaw), |
|
ToRad(rollRate), ToRad(pitchRate), ToRad(yawRate)); |
|
|
|
#endif |
|
|
|
static uint32_t last_report; |
|
uint32_t tnow = millis(); |
|
extern AP_DCM dcm; |
|
Vector3f omega = dcm.get_gyro(); |
|
// report roll/pitch discrepancies |
|
if (tnow - last_report > 5000 || |
|
(tnow - last_report > 1000 && |
|
(fabs(roll - dcm.roll_sensor/100.0) > 5.0 || |
|
fabs(pitch - dcm.pitch_sensor/100.0) > 5.0))) { |
|
last_report = tnow; |
|
printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n", |
|
roll, dcm.roll_sensor/100.0, |
|
pitch, dcm.pitch_sensor/100.0, |
|
rollRate, ToDeg(omega.x), |
|
pitchRate, ToDeg(omega.y)); |
|
} |
|
} |
|
|
|
|
|
/* |
|
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); |
|
}
|
|
|