aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

desktop: improved the accuracy of the sensor emulation

Browse Source
This commit is contained in:
Andrew Tridgell 2011-11-28 16:01:23 +11:00
parent ff727e87ca
commit 210536096f
5 changed files with 95 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/Desktop/support/sitl.cpp
View File

@ -41,7 +41,16 @@ struct ADC_UDR2 UDR2;
struct RC_ICR4 ICR4;
extern AP_TimerProcess timer_scheduler;
extern Arduino_Mega_ISR_Registry isr_registry;
static volatile uint32_t sim_input_count;
static volatile struct {
double latitude, longitude; // degrees
double altitude; // MSL
double heading; // degrees
double speedN, speedE; // m/s
double roll, pitch, yaw; // degrees
double airspeed; // m/s
uint32_t update_count;
} sim_state;
/*
@ -123,14 +132,19 @@ static void sitl_fgear_input(void)
return;
}
sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude,
ft2m(d.fg_pkt.altitude),
ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE), true);
sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed));
sitl_update_barometer(ft2m(d.fg_pkt.altitude));
sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading);
sim_state.latitude = d.fg_pkt.latitude;
sim_state.longitude = d.fg_pkt.longitude;
sim_state.altitude = ft2m(d.fg_pkt.altitude);
sim_state.speedN = ft2m(d.fg_pkt.speedN);
sim_state.speedE = ft2m(d.fg_pkt.speedE);
sim_state.roll = d.fg_pkt.rollDeg;
sim_state.pitch = d.fg_pkt.pitchDeg;
sim_state.yaw = d.fg_pkt.yawDeg;
sim_state.heading = d.fg_pkt.heading;
sim_state.airspeed = kt2mps(d.fg_pkt.airspeed);
sim_state.update_count++;
count++;
sim_input_count++;
if (millis() - last_report > 1000) {
printf("SIM %u FPS\n", count);
count = 0;
@ -239,6 +253,8 @@ static void sitl_simulator_output(void)
*/
static void timer_handler(int signum)
{
static uint32_t last_update_count;
/* make sure we die if our parent dies */
if (kill(parent_pid, 0) != 0) {
exit(1);
@ -258,9 +274,22 @@ static void timer_handler(int signum)
// send RC output to flight sim
sitl_simulator_output();
if (sim_input_count == 0) {
if (sim_state.update_count == 0) {
sitl_update_gps(0, 0, 0, 0, 0, false);
return;
}
if (sim_state.update_count == last_update_count) {
return;
}
last_update_count = sim_state.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.roll, sim_state.pitch, sim_state.heading, sim_state.airspeed);
sitl_update_barometer(sim_state.altitude);
sitl_update_compass(sim_state.heading, sim_state.roll, sim_state.pitch, sim_state.heading);
}

44
libraries/Desktop/support/sitl_adc.cpp
View File

@ -52,16 +52,21 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
const float _gyro_gain_y = 0.41;
const float _gyro_gain_z = 0.41;
const float _accel_scale = 9.80665 / 423.8;
uint16_t adc[7];
float adc[7];
float xAccel, yAccel, zAccel, scale;
float rollRate, pitchRate, yawRate;
static uint32_t last_update;
static float last_roll, last_pitch, last_yaw;
unsigned long delta_t;
// 200Hz max
if (micros() - last_update < 5000) {
return;
}
/* map roll/pitch/yaw to values the accelerometer would see */
xAccel = sin(ToRad(pitch));
yAccel = -sin(ToRad(roll));
xAccel = sin(ToRad(pitch)) * cos(ToRad(roll));
yAccel = -sin(ToRad(roll)) * cos(ToRad(pitch));
zAccel = -cos(ToRad(roll)) * cos(ToRad(pitch));
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel));
xAccel *= scale;
@ -76,12 +81,13 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
delta_t = micros();
} else {
delta_t = micros() - last_update;
rollRate = 1.0e6 * (roll - last_roll) / delta_t;
pitchRate = 1.0e6 * (pitch - last_pitch) / delta_t;
yawRate = 1.0e6 * (yaw - last_yaw) / delta_t;
rollRate = normalise(rollRate, -180, 180);
pitchRate = normalise(pitchRate, -180, 180);
yawRate = normalise(yawRate, -180, 180);
float rollChange, pitchChange, yawChange;
rollChange = normalise180(roll - last_roll);
pitchChange = normalise180(pitch - last_pitch);
yawChange = normalise180(yaw - last_yaw);
rollRate = 1.0e6 * rollChange / delta_t;
pitchRate = 1.0e6 * pitchChange / delta_t;
yawRate = 1.0e6 * yawChange / delta_t;
}
last_update += delta_t;
@ -100,20 +106,30 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
/* 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]<<3) & 0x7FFF);
UDR2.set(sensor_map[i], adc[i]);
}
// set the airspeed sensor input
UDR2.set(7, airspeed_sensor(airspeed)<<3);
UDR2.set(7, airspeed_sensor(airspeed));
/* FIX: rubbish value for temperature for now */
UDR2.set(3, 2000<<3);
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
#if 0
extern AP_DCM dcm;
printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f rollRate=%6.1f pitchRate=%6.1f\n",
roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate);
Vector3f omega = dcm.get_gyro();
printf("dt=%5u adc[2]=%6.1f roll=%6.1f / %6.1f yaw=%6.1f / %6.1f yawRate=%6.3f / %6.3f\n",
(unsigned)delta_t,
adc[2],
roll, dcm.roll_sensor/100.0, yaw, dcm.yaw_sensor/100.0, yawRate, ToDeg(omega.z));
#endif
}

33
libraries/Desktop/support/sitl_adc.h
View File

@ -6,11 +6,13 @@
#ifndef _SITL_ADC_H
#define _SITL_ADC_H
#include <stdlib.h>
// this implements the UDR2 register
struct ADC_UDR2 {
uint16_t value, next_value;
uint8_t idx;
uint16_t channels[8];
float channels[8];
ADC_UDR2() {
// constructor
@ -26,19 +28,26 @@ struct ADC_UDR2 {
to output next
*/
ADC_UDR2& operator=(uint8_t cmd) {
float next_analog;
switch (cmd) {
case 0x87: next_value = channels[0]; break;
case 0xC7: next_value = channels[1]; break;
case 0x97: next_value = channels[2]; break;
case 0xD7: next_value = channels[3]; break;
case 0xA7: next_value = channels[4]; break;
case 0xE7: next_value = channels[5]; break;
case 0xB7: next_value = channels[6]; break;
case 0xF7: next_value = channels[7]; break;
case 0x87: next_analog = channels[0]; break;
case 0xC7: next_analog = channels[1]; break;
case 0x97: next_analog = channels[2]; break;
case 0xD7: next_analog = channels[3]; break;
case 0xA7: next_analog = channels[4]; break;
case 0xE7: next_analog = channels[5]; break;
case 0xB7: next_analog = channels[6]; break;
case 0xF7: next_analog = channels[7]; break;
case 0:
default: return *this;
}
if (cmd != 0) {
idx = 1;
idx = 1;
int noise = (((unsigned long)random()) % 3) - 1;
next_value = (unsigned)(next_analog + noise + 0.5);
if (next_value >= 0x1000) {
next_value = 0xFFF;
}
next_value = (next_value << 3);
return *this;
}
@ -60,7 +69,7 @@ struct ADC_UDR2 {
/*
interface to set a channel value from SITL
*/
void set(uint8_t i, uint16_t v) {
void set(uint8_t i, float v) {
channels[i] = v;
}
};

5
libraries/Desktop/support/util.cpp
View File

@ -90,3 +90,8 @@ double normalise(double v, double min, double max)
}
return v;
}
double normalise180(double v)
{
return normalise(v, -180, 180);
}

1
libraries/Desktop/support/util.h
View File

@ -9,3 +9,4 @@ float swap_float(float f);
void swap_floats(float *f, unsigned count);
void set_nonblocking(int fd);
double normalise(double v, double min, double max);
double normalise180(double v);