mirror of https://github.com/ArduPilot/ardupilot
131 lines
3.6 KiB
C++
131 lines
3.6 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_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);
|
|
}
|