mirror of https://github.com/ArduPilot/ardupilot
desktop: first version of register level SITL support
this adds register level emulation of the ADS7844 and the RC input/output hardware on the APM1, allowing for SITL testing without enabling HIL in the code
This commit is contained in:
Andrew Tridgell
Pat Hickey
parent
f071321efa
commit
2bed1dcfd5
|
|
@ -156,8 +156,13 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
|||
AP_ADC_ADS7844 adc;
|
||||
#endif
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
APM_BMP085_HIL_Class barometer;
|
||||
AP_Compass_HIL compass;
|
||||
#else
|
||||
APM_BMP085_Class barometer;
|
||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
#endif
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
|
|
|
|||
|
|
@ -132,8 +132,14 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
|||
|
||||
// real sensors
|
||||
static AP_ADC_ADS7844 adc;
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
APM_BMP085_HIL_Class barometer;
|
||||
AP_Compass_HIL compass;
|
||||
#else
|
||||
static APM_BMP085_Class barometer;
|
||||
static AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
#endif
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
|
|
|
|||
|
|
@ -185,7 +185,7 @@ else
|
|||
endif
|
||||
|
||||
# these are library objects we don't want in the desktop build (maybe we'll add them later)
|
||||
NODESKTOP := DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp AP_InertialSensor_Oilpan.cpp AP_InertialSensor/AP_InertialSensor.cpp
|
||||
NODESKTOP := DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp
|
||||
|
||||
#
|
||||
# Find sketchbook libraries referenced by the sketch.
|
||||
|
|
|
|||
|
|
@ -834,7 +834,12 @@
|
|||
#define TCNT4H _SFR_MEM8(0xA5)
|
||||
|
||||
/* Combine ICR4L and ICR4H */
|
||||
#ifdef DESKTOP_BUILD
|
||||
#include "../support/sitl_rc.h"
|
||||
extern struct RC_ICR4 ICR4;
|
||||
#else
|
||||
#define ICR4 _SFR_MEM16(0xA6)
|
||||
#endif
|
||||
|
||||
#define ICR4L _SFR_MEM8(0xA6)
|
||||
#define ICR4H _SFR_MEM8(0xA7)
|
||||
|
|
@ -1061,7 +1066,12 @@
|
|||
# define UBRR2L _SFR_MEM8(0xD4)
|
||||
# define UBRR2H _SFR_MEM8(0xD5)
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
#include "../support/sitl_adc.h"
|
||||
extern struct ADC_UDR2 UDR2;
|
||||
#else
|
||||
# define UDR2 _SFR_MEM8(0XD6)
|
||||
#endif
|
||||
|
||||
#endif /* __ATmegaxx0__ */
|
||||
|
||||
|
|
|
|||
|
|
@ -38,9 +38,14 @@ long unsigned int micros(void)
|
|||
(desktop_state.sketch_start_time.tv_usec*1.0e-6)));
|
||||
}
|
||||
|
||||
void delayMicroseconds(unsigned usec)
|
||||
{
|
||||
usleep(usec);
|
||||
}
|
||||
|
||||
void delay(long unsigned msec)
|
||||
{
|
||||
usleep(msec*1000);
|
||||
delayMicroseconds(msec*1000);
|
||||
}
|
||||
|
||||
size_t strlcat_P(char *d, PGM_P s, size_t bufsize)
|
||||
|
|
|
|||
|
|
@ -38,7 +38,6 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
|
|
@ -87,13 +86,6 @@ static void tcp_start_connection(unsigned int serial_port, bool wait_for_connect
|
|||
|
||||
s->serial_port = serial_port;
|
||||
|
||||
/* Create the listening socket */
|
||||
s->listen_fd = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (s->listen_fd == -1) {
|
||||
fprintf(stderr, "ECHOSERV: Error creating listening socket - %s\n", strerror(errno));
|
||||
exit(1);
|
||||
}
|
||||
|
||||
memset(&sockaddr,0,sizeof(sockaddr));
|
||||
|
||||
#ifdef HAVE_SOCK_SIN_LEN
|
||||
|
|
@ -114,7 +106,7 @@ static void tcp_start_connection(unsigned int serial_port, bool wait_for_connect
|
|||
ret = bind(s->listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
|
||||
if (ret == -1) {
|
||||
fprintf(stderr, "bind failed on port %u - %s\n",
|
||||
LISTEN_BASE_PORT+serial_port,
|
||||
(unsigned)ntohs(sockaddr.sin_port),
|
||||
strerror(errno));
|
||||
exit(1);
|
||||
}
|
||||
|
|
@ -208,7 +200,22 @@ FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volati
|
|||
|
||||
void FastSerial::begin(long baud)
|
||||
{
|
||||
tcp_start_connection(_u2x, _u2x == 0?true:false);
|
||||
switch (_u2x) {
|
||||
case 0:
|
||||
tcp_start_connection(_u2x, true);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
/* gps */
|
||||
tcp_state[1].connected = true;
|
||||
tcp_state[1].fd = sitl_gps_pipe();
|
||||
tcp_state[1].serial_port = 1;
|
||||
break;
|
||||
|
||||
default:
|
||||
tcp_start_connection(_u2x, false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace)
|
||||
|
|
@ -268,6 +275,13 @@ int FastSerial::read(void)
|
|||
return -1;
|
||||
}
|
||||
|
||||
if (s->serial_port == 1) {
|
||||
if (sitl_gps_read(s->fd, &c, 1) == 1) {
|
||||
return (uint8_t)c;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int n = recv(s->fd, &c, 1, MSG_DONTWAIT | MSG_NOSIGNAL);
|
||||
if (n <= 0) {
|
||||
// the socket has reached EOF
|
||||
|
|
@ -278,7 +292,7 @@ int FastSerial::read(void)
|
|||
return -1;
|
||||
}
|
||||
if (n == 1) {
|
||||
return (int)c;
|
||||
return (uint8_t)c;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,10 +1,24 @@
|
|||
#ifndef _DESKTOP_H
|
||||
#define _DESKTOP_H
|
||||
|
||||
struct desktop_state {
|
||||
struct desktop_info {
|
||||
bool slider; // slider switch state, True means CLI mode
|
||||
struct timeval sketch_start_time;
|
||||
bool quadcopter; // use quadcopter outputs
|
||||
};
|
||||
|
||||
extern struct desktop_state desktop_state;
|
||||
extern struct desktop_info desktop_state;
|
||||
|
||||
void desktop_serial_select_setup(fd_set *fds, int *fd_high);
|
||||
void sitl_input(void);
|
||||
void sitl_setup(void);
|
||||
int sitl_gps_pipe(void);
|
||||
ssize_t sitl_gps_read(int fd, void *buf, size_t count);
|
||||
void sitl_update_compass(float heading, float roll, float pitch, float yaw);
|
||||
void sitl_update_gps(float latitude, float longitude, float altitude,
|
||||
float speedN, float speedE);
|
||||
void sitl_update_adc(float roll, float pitch, float yaw, float airspeed);
|
||||
void sitl_setup_adc(void);
|
||||
void sitl_update_barometer(float altitude);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,13 +6,14 @@
|
|||
#include <wiring.h>
|
||||
#include <getopt.h>
|
||||
#include <signal.h>
|
||||
#include <string.h>
|
||||
#include "desktop.h"
|
||||
|
||||
void setup(void);
|
||||
void loop(void);
|
||||
|
||||
// the state of the desktop simulation
|
||||
struct desktop_state desktop_state;
|
||||
struct desktop_info desktop_state;
|
||||
|
||||
static void usage(void)
|
||||
{
|
||||
|
|
@ -21,16 +22,9 @@ static void usage(void)
|
|||
printf("\t-w wipe eeprom and dataflash\n");
|
||||
}
|
||||
|
||||
void sig_alarm(int sig)
|
||||
{
|
||||
printf("alarm signal in desktop emulation - loop not running\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int main(int argc, char * const argv[])
|
||||
{
|
||||
int opt;
|
||||
|
||||
// default state
|
||||
desktop_state.slider = false;
|
||||
gettimeofday(&desktop_state.sketch_start_time, NULL);
|
||||
|
|
@ -50,9 +44,11 @@ int main(int argc, char * const argv[])
|
|||
}
|
||||
}
|
||||
|
||||
signal(SIGALRM, sig_alarm);
|
||||
if (strcmp(SKETCH, "ArduCopter") == 0) {
|
||||
desktop_state.quadcopter = true;
|
||||
}
|
||||
|
||||
// run main setup() function from sketch
|
||||
sitl_setup();
|
||||
setup();
|
||||
|
||||
while (true) {
|
||||
|
|
@ -60,26 +56,14 @@ int main(int argc, char * const argv[])
|
|||
fd_set fds;
|
||||
int fd_high = 0;
|
||||
|
||||
if (!desktop_state.slider) {
|
||||
alarm(5);
|
||||
}
|
||||
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(0, &fds);
|
||||
loop();
|
||||
|
||||
desktop_serial_select_setup(&fds, &fd_high);
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 100;
|
||||
|
||||
if (select(fd_high+1, &fds, NULL, NULL, &tv) > 0 &&
|
||||
FD_ISSET(0, &fds)) {
|
||||
char c;
|
||||
if (read(0, &c, 1) != 1) {
|
||||
// EOF on stdin
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
select(fd_high+1, &fds, NULL, NULL, &tv);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,299 @@
|
|||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates the APM1 hardware sufficiently for the APM code to
|
||||
think it is running on real hardware
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/udp.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
#include <signal.h>
|
||||
#include <math.h>
|
||||
#include <APM_RC.h>
|
||||
#include <wiring.h>
|
||||
#include <AP_PeriodicProcess.h>
|
||||
#include <AP_TimerProcess.h>
|
||||
#include <AP_TimerAperiodicProcess.h>
|
||||
#include "sitl_adc.h"
|
||||
#include "sitl_rc.h"
|
||||
#include "desktop.h"
|
||||
#include "util.h"
|
||||
|
||||
#define FGIN_PORT 5501
|
||||
#define FGOUT_PORT 5502
|
||||
|
||||
static int sitl_fd;
|
||||
struct sockaddr_in fgout_addr;
|
||||
static pid_t parent_pid;
|
||||
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;
|
||||
|
||||
|
||||
/*
|
||||
setup a FGear listening UDP port, using protocol from MAVLink.xml
|
||||
*/
|
||||
static void setup_fgear(void)
|
||||
{
|
||||
int one=1, ret;
|
||||
struct sockaddr_in sockaddr;
|
||||
|
||||
memset(&sockaddr,0,sizeof(sockaddr));
|
||||
|
||||
#ifdef HAVE_SOCK_SIN_LEN
|
||||
sockaddr.sin_len = sizeof(sockaddr);
|
||||
#endif
|
||||
sockaddr.sin_port = htons(FGIN_PORT);
|
||||
sockaddr.sin_family = AF_INET;
|
||||
|
||||
sitl_fd = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sitl_fd == -1) {
|
||||
fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno));
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/* we want to be able to re-use ports quickly */
|
||||
setsockopt(sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
|
||||
|
||||
ret = bind(sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
|
||||
if (ret == -1) {
|
||||
fprintf(stderr, "SITL: bind failed on port %u - %s\n",
|
||||
(unsigned)ntohs(sockaddr.sin_port), strerror(errno));
|
||||
exit(1);
|
||||
}
|
||||
|
||||
set_nonblocking(sitl_fd);
|
||||
}
|
||||
|
||||
/*
|
||||
check for a fgear packet
|
||||
*/
|
||||
static void sitl_fgear_input(void)
|
||||
{
|
||||
ssize_t size;
|
||||
struct fg_mavlink {
|
||||
double latitude, longitude, altitude, heading,
|
||||
speedN, speedE,
|
||||
xAccel, yAccel, zAccel,
|
||||
rollRate, pitchRate, yawRate,
|
||||
rollDeg, pitchDeg, yawDeg,
|
||||
airspeed;
|
||||
uint32_t magic;
|
||||
};
|
||||
struct pwm_packet {
|
||||
uint16_t pwm[8];
|
||||
};
|
||||
union {
|
||||
struct fg_mavlink fg_pkt;
|
||||
struct pwm_packet pwm_pkt;
|
||||
} d;
|
||||
|
||||
size = recv(sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
|
||||
switch (size) {
|
||||
case 132:
|
||||
static uint32_t last_report;
|
||||
static uint32_t count;
|
||||
|
||||
/* sigh, its big-endian */
|
||||
swap_doubles(&d.fg_pkt.latitude, 16);
|
||||
d.fg_pkt.magic = ntohl(d.fg_pkt.magic);
|
||||
if (d.fg_pkt.magic != 0x4c56414d) {
|
||||
printf("Bad fgear packet - magic=0x%08x\n", d.fg_pkt.magic);
|
||||
return;
|
||||
}
|
||||
|
||||
if (d.fg_pkt.latitude == 0 ||
|
||||
d.fg_pkt.longitude == 0 ||
|
||||
d.fg_pkt.altitude <= 0) {
|
||||
// garbage input
|
||||
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));
|
||||
sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.yawDeg, 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.yawDeg);
|
||||
count++;
|
||||
sim_input_count++;
|
||||
if (millis() - last_report > 1000) {
|
||||
printf("SIM %u FPS\n", count);
|
||||
count = 0;
|
||||
last_report = millis();
|
||||
}
|
||||
break;
|
||||
|
||||
case 16: {
|
||||
// a packet giving the receiver PWM inputs
|
||||
uint8_t i;
|
||||
for (i=0; i<8; i++) {
|
||||
// setup the ICR4 register for the RC channel
|
||||
// inputs
|
||||
if (d.pwm_pkt.pwm[i] != 0) {
|
||||
ICR4.set(i, d.pwm_pkt.pwm[i]);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
send RC outputs to simulator for a quadcopter
|
||||
*/
|
||||
static void sitl_quadcopter_output(uint16_t pwm[8])
|
||||
{
|
||||
struct fg_output {
|
||||
float throttle[4];
|
||||
uint16_t pwm[8];
|
||||
} pkt;
|
||||
for (uint8_t i=0; i<8; i++) {
|
||||
pkt.pwm[i] = htonl(pwm[i]);
|
||||
}
|
||||
for (uint8_t i=0; i<4; i++) {
|
||||
pkt.throttle[i] = swap_float((pwm[i]-1000) / 1000.0);
|
||||
}
|
||||
sendto(sitl_fd, &pkt, sizeof(pkt), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr));
|
||||
}
|
||||
|
||||
/*
|
||||
send RC outputs to simulator for a plane
|
||||
*/
|
||||
static void sitl_plane_output(uint16_t pwm[8])
|
||||
{
|
||||
double servo[4];
|
||||
|
||||
servo[0] = (((int)pwm[0]) - 1500)/500.0;
|
||||
servo[1] = (((int)pwm[1]) - 1500)/500.0;
|
||||
servo[2] = (((int)pwm[3]) - 1500)/500.0;
|
||||
servo[3] = (pwm[2] - 1000) / 1000.0;
|
||||
swap_doubles(servo, 4);
|
||||
sendto(sitl_fd, &servo, sizeof(servo), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send RC outputs to simulator for a quadcopter
|
||||
*/
|
||||
static void sitl_simulator_output(void)
|
||||
{
|
||||
static uint32_t last_update;
|
||||
uint16_t pwm[8];
|
||||
/* this maps the registers used for PWM outputs. The RC
|
||||
* driver updates these whenever it wants the channel output
|
||||
* to change */
|
||||
uint16_t *reg[11] = { &OCR5B, &OCR5C, &OCR1B, &OCR1C,
|
||||
&OCR4C, &OCR4B, &OCR3C, &OCR3B,
|
||||
&OCR5A, &OCR1A, &OCR3A };
|
||||
uint8_t i;
|
||||
|
||||
if (last_update == 0) {
|
||||
(*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2;
|
||||
(*reg[2]) = (*reg[4]) = (*reg[6]) = 1000*2;
|
||||
(*reg[5]) = (*reg[7]) = 1800*2;
|
||||
}
|
||||
|
||||
if (millis() - last_update < 50) {
|
||||
// output to simulator at 20Hz
|
||||
return;
|
||||
}
|
||||
last_update = millis();
|
||||
|
||||
for (i=0; i<8; i++) {
|
||||
// the registers are 2x the PWM value
|
||||
pwm[i] = (*reg[i])/2;
|
||||
}
|
||||
|
||||
if (desktop_state.quadcopter) {
|
||||
sitl_quadcopter_output(pwm);
|
||||
} else {
|
||||
sitl_plane_output(pwm);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
timer called at 1kHz
|
||||
*/
|
||||
static void timer_handler(int signum)
|
||||
{
|
||||
/* make sure we die if our parent dies */
|
||||
if (kill(parent_pid, 0) != 0) {
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/* check for packet from flight sim */
|
||||
sitl_fgear_input();
|
||||
|
||||
// trigger all timers
|
||||
timer_scheduler.run();
|
||||
|
||||
// trigger RC input
|
||||
if (isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]) {
|
||||
isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]();
|
||||
}
|
||||
|
||||
// send RC output to flight sim
|
||||
sitl_simulator_output();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
setup a timer used to prod the ISRs
|
||||
*/
|
||||
static void setup_timer(void)
|
||||
{
|
||||
struct itimerval it;
|
||||
struct sigaction act;
|
||||
|
||||
act.sa_handler = timer_handler;
|
||||
act.sa_flags = SA_RESTART|SA_NODEFER;
|
||||
sigemptyset(&act.sa_mask);
|
||||
sigaddset(&act.sa_mask, SIGALRM);
|
||||
sigaction(SIGALRM, &act, NULL);
|
||||
|
||||
it.it_interval.tv_sec = 0;
|
||||
it.it_interval.tv_usec = 1000; // 1KHz
|
||||
it.it_value = it.it_interval;
|
||||
|
||||
setitimer(ITIMER_REAL, &it, NULL);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
setup for SITL handling
|
||||
*/
|
||||
void sitl_setup(void)
|
||||
{
|
||||
parent_pid = getppid();
|
||||
|
||||
fgout_addr.sin_family = AF_INET;
|
||||
fgout_addr.sin_port = htons(FGOUT_PORT);
|
||||
inet_pton(AF_INET, "127.0.0.1", &fgout_addr.sin_addr);
|
||||
|
||||
setup_timer();
|
||||
setup_fgear();
|
||||
sitl_setup_adc();
|
||||
printf("Starting SITL input\n");
|
||||
|
||||
// wait until the first valid sim packet has
|
||||
// been processed, to ensure the sensor hardware
|
||||
// has sane values
|
||||
while (sim_input_count == 0) /* noop */ ;
|
||||
}
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/*
|
||||
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 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 = 0.4;
|
||||
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 xAccel, yAccel, zAccel, scale;
|
||||
float rollRate, pitchRate, yawRate;
|
||||
static uint32_t last_update;
|
||||
static float last_roll, last_pitch, last_yaw;
|
||||
uint32_t delta_t;
|
||||
|
||||
/* map roll/pitch/yaw to values the accelerometer would see */
|
||||
xAccel = sin(ToRad(pitch));
|
||||
yAccel = -sin(ToRad(roll));
|
||||
zAccel = -cos(ToRad(roll)) * cos(ToRad(pitch));
|
||||
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel));
|
||||
xAccel *= scale;
|
||||
yAccel *= scale;
|
||||
zAccel *= scale;
|
||||
|
||||
/* map roll/pitch/yaw to values the gyro would see */
|
||||
if (last_update == 0) {
|
||||
rollRate = 0;
|
||||
pitchRate = 0;
|
||||
yawRate = 0;
|
||||
delta_t = millis();
|
||||
} else {
|
||||
delta_t = millis() - last_update;
|
||||
rollRate = 1000.0 * (roll - last_roll) / delta_t;
|
||||
pitchRate = 1000.0 * (pitch - last_pitch) / delta_t;
|
||||
yawRate = 1000.0 * (yaw - last_yaw) / delta_t;
|
||||
}
|
||||
last_update += delta_t;
|
||||
|
||||
last_roll = roll;
|
||||
last_pitch = pitch;
|
||||
last_yaw = yaw;
|
||||
|
||||
/* work out the ADC channel values */
|
||||
adc[0] = (rollRate / (_gyro_gain_x * _sensor_signs[0])) + gyro_offset;
|
||||
adc[1] = (pitchRate / (_gyro_gain_y * _sensor_signs[1])) + gyro_offset;
|
||||
adc[2] = (yawRate / (_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]<<3) & 0x7FFF);
|
||||
}
|
||||
|
||||
|
||||
// set the airspeed sensor input
|
||||
UDR2.set(7, airspeed_sensor(airspeed)<<3);
|
||||
|
||||
/* FIX: rubbish value for temperature for now */
|
||||
UDR2.set(3, 2000<<3);
|
||||
|
||||
#if 0
|
||||
extern AP_DCM dcm;
|
||||
printf("roll=%6.1f pitch=%6.1f dcm_roll=%6.1f dcm_pitch=%6.1f rollRate=%6.3f pitchRate=%6.3f\n",
|
||||
roll, pitch, dcm.roll_sensor/100.0, dcm.pitch_sensor/100.0, rollRate, pitchRate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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);
|
||||
}
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
/*
|
||||
ADS7844 register emulation
|
||||
Code by Andrew Tridgell November 2011
|
||||
*/
|
||||
|
||||
#ifndef _SITL_ADC_H
|
||||
#define _SITL_ADC_H
|
||||
|
||||
// this implements the UDR2 register
|
||||
struct ADC_UDR2 {
|
||||
uint16_t value, next_value;
|
||||
uint8_t idx;
|
||||
uint16_t channels[8];
|
||||
|
||||
ADC_UDR2() {
|
||||
// constructor
|
||||
for (uint8_t i=0; i<8; i++) {
|
||||
channels[i] = 0xFFFF;
|
||||
}
|
||||
value = next_value = 0;
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
assignment of UDR2 selects which ADC channel
|
||||
to output next
|
||||
*/
|
||||
ADC_UDR2& operator=(uint8_t cmd) {
|
||||
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;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*
|
||||
read from UDR2 fetches a byte from the channel
|
||||
*/
|
||||
operator int() {
|
||||
switch (idx) {
|
||||
case 0: idx++; return(value>>8);
|
||||
case 1: idx++; return(value&0xFF);
|
||||
}
|
||||
value = next_value;
|
||||
idx=0;
|
||||
return(value>>8);
|
||||
}
|
||||
|
||||
/*
|
||||
interface to set a channel value from SITL
|
||||
*/
|
||||
void set(uint8_t i, uint16_t v) {
|
||||
channels[i] = v;
|
||||
}
|
||||
};
|
||||
|
||||
#endif // _SITL_ADC_H
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a barometer
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#include "desktop.h"
|
||||
#include "util.h"
|
||||
|
||||
/*
|
||||
setup the barometer with new input
|
||||
altitude is in meters
|
||||
*/
|
||||
void sitl_update_barometer(float altitude)
|
||||
{
|
||||
extern APM_BMP085_HIL_Class barometer;
|
||||
double Temp, Press, y;
|
||||
|
||||
Temp = 312;
|
||||
|
||||
y = ((altitude-584.0) * 1000.0) / 29271.267;
|
||||
y /= (Temp / 10.0) + 273.15;
|
||||
y = 1.0/exp(y);
|
||||
y *= 95446.0;
|
||||
|
||||
Press = y;
|
||||
|
||||
barometer.setHIL(Temp, Press);
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a compass
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Compass.h>
|
||||
#include "desktop.h"
|
||||
#include "util.h"
|
||||
|
||||
/*
|
||||
given a magnetic heading, and roll, pitch, yaw values,
|
||||
calculate consistent magnetometer components
|
||||
|
||||
All angles are in radians
|
||||
*/
|
||||
static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw)
|
||||
{
|
||||
Vector3f v;
|
||||
double headX, headY, cos_roll, sin_roll, cos_pitch, sin_pitch, scale;
|
||||
const double magnitude = 665;
|
||||
|
||||
cos_roll = cos(roll);
|
||||
sin_roll = sin(roll);
|
||||
cos_pitch = cos(pitch);
|
||||
sin_pitch = sin(pitch);
|
||||
|
||||
|
||||
headY = -sin(heading);
|
||||
headX = cos(heading);
|
||||
|
||||
if (fabs(cos_roll) < 1.0e-20) {
|
||||
cos_roll = 1.0e-10;
|
||||
}
|
||||
if (fabs(cos_pitch) < 1.0e-20) {
|
||||
cos_pitch = 1.0e-10;
|
||||
}
|
||||
|
||||
v.z = 0;
|
||||
v.y = (headY + v.z*sin_roll) / cos_roll;
|
||||
v.x = (headX - (v.y*sin_roll*sin_pitch + v.z*cos_roll*sin_pitch)) / cos_pitch;
|
||||
scale = magnitude / sqrt((v.x*v.x) + (v.y*v.y) + (v.z*v.z));
|
||||
v *= scale;
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
setup the compass with new input
|
||||
all inputs are in degrees
|
||||
*/
|
||||
void sitl_update_compass(float heading, float roll, float pitch, float yaw)
|
||||
{
|
||||
extern AP_Compass_HIL compass;
|
||||
Vector3f m = heading_to_mag(ToRad(heading),
|
||||
ToRad(roll),
|
||||
ToRad(pitch),
|
||||
ToRad(yaw));
|
||||
compass.setHIL(m.x, m.y, m.z);
|
||||
}
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a GPS on a serial port
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_GPS_UBLOX.h>
|
||||
#include "desktop.h"
|
||||
#include "util.h"
|
||||
|
||||
// state of GPS emulation
|
||||
static struct {
|
||||
/* pipe emulating UBLOX GPS serial stream */
|
||||
int gps_fd;
|
||||
uint32_t last_update; // milliseconds
|
||||
} gps_state;
|
||||
|
||||
/*
|
||||
hook for reading from the GPS pipe
|
||||
*/
|
||||
ssize_t sitl_gps_read(int fd, void *buf, size_t count)
|
||||
{
|
||||
return read(fd, buf, count);
|
||||
}
|
||||
|
||||
/*
|
||||
setup GPS input pipe
|
||||
*/
|
||||
int sitl_gps_pipe(void)
|
||||
{
|
||||
int fd[2];
|
||||
pipe(fd);
|
||||
gps_state.gps_fd = fd[1];
|
||||
gps_state.last_update = millis();
|
||||
set_nonblocking(gps_state.gps_fd);
|
||||
set_nonblocking(fd[0]);
|
||||
return fd[0];
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send a UBLOX GPS message
|
||||
*/
|
||||
static void gps_send(uint8_t msgid, uint8_t *buf, uint16_t size)
|
||||
{
|
||||
const uint8_t PREAMBLE1 = 0xb5;
|
||||
const uint8_t PREAMBLE2 = 0x62;
|
||||
const uint8_t CLASS_NAV = 0x1;
|
||||
uint8_t hdr[6], chk[2];
|
||||
hdr[0] = PREAMBLE1;
|
||||
hdr[1] = PREAMBLE2;
|
||||
hdr[2] = CLASS_NAV;
|
||||
hdr[3] = msgid;
|
||||
hdr[4] = size & 0xFF;
|
||||
hdr[5] = size >> 8;
|
||||
chk[0] = chk[1] = hdr[2];
|
||||
chk[1] += (chk[0] += hdr[3]);
|
||||
chk[1] += (chk[0] += hdr[4]);
|
||||
chk[1] += (chk[0] += hdr[5]);
|
||||
for (uint8_t i=0; i<size; i++) {
|
||||
chk[1] += (chk[0] += buf[i]);
|
||||
}
|
||||
write(gps_state.gps_fd, hdr, sizeof(hdr));
|
||||
write(gps_state.gps_fd, buf, size);
|
||||
write(gps_state.gps_fd, chk, sizeof(chk));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
possibly send a new GPS UBLOX packet
|
||||
*/
|
||||
void sitl_update_gps(float latitude, float longitude, float altitude,
|
||||
float speedN, float speedE)
|
||||
{
|
||||
struct ubx_nav_posllh {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t longitude;
|
||||
int32_t latitude;
|
||||
int32_t altitude_ellipsoid;
|
||||
int32_t altitude_msl;
|
||||
uint32_t horizontal_accuracy;
|
||||
uint32_t vertical_accuracy;
|
||||
} pos;
|
||||
struct ubx_nav_status {
|
||||
uint32_t time; // GPS msToW
|
||||
uint8_t fix_type;
|
||||
uint8_t fix_status;
|
||||
uint8_t differential_status;
|
||||
uint8_t res;
|
||||
uint32_t time_to_first_fix;
|
||||
uint32_t uptime; // milliseconds
|
||||
} status;
|
||||
struct ubx_nav_velned {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t ned_north;
|
||||
int32_t ned_east;
|
||||
int32_t ned_down;
|
||||
uint32_t speed_3d;
|
||||
uint32_t speed_2d;
|
||||
int32_t heading_2d;
|
||||
uint32_t speed_accuracy;
|
||||
uint32_t heading_accuracy;
|
||||
} velned;
|
||||
const uint8_t MSG_POSLLH = 0x2;
|
||||
const uint8_t MSG_STATUS = 0x3;
|
||||
const uint8_t MSG_VELNED = 0x12;
|
||||
float lon_scale;
|
||||
|
||||
// 4Hz
|
||||
if (millis() - gps_state.last_update < 250) {
|
||||
return;
|
||||
}
|
||||
gps_state.last_update = millis();
|
||||
|
||||
pos.time = millis(); // FIX
|
||||
pos.longitude = longitude * 1.0e7;
|
||||
pos.latitude = latitude * 1.0e7;
|
||||
pos.altitude_ellipsoid = altitude*1000.0;
|
||||
pos.altitude_msl = altitude*1000.0;
|
||||
pos.horizontal_accuracy = 5;
|
||||
pos.vertical_accuracy = 10;
|
||||
|
||||
status.time = pos.time;
|
||||
status.fix_type = 3;
|
||||
status.fix_status = 1;
|
||||
status.differential_status = 0;
|
||||
status.res = 0;
|
||||
status.time_to_first_fix = 0;
|
||||
status.uptime = millis();
|
||||
|
||||
velned.time = pos.time;
|
||||
velned.ned_north = 100.0 * speedN;
|
||||
velned.ned_east = 100.0 * speedE;
|
||||
velned.ned_down = 0;
|
||||
#define sqr(x) ((x)*(x))
|
||||
velned.speed_2d = sqrt(sqr(speedN)+sqr(speedE)) * 100;
|
||||
velned.speed_3d = velned.speed_2d;
|
||||
lon_scale = cos(ToRad(latitude));
|
||||
velned.heading_2d = ToDeg(atan2(lon_scale*speedE, speedN)) * 100000.0;
|
||||
velned.speed_accuracy = 2;
|
||||
velned.heading_accuracy = 4;
|
||||
|
||||
if (gps_state.gps_fd == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
|
||||
gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status));
|
||||
gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/*
|
||||
RC input emulation
|
||||
Code by Andrew Tridgell November 2011
|
||||
*/
|
||||
|
||||
#ifndef _SITL_RC_H
|
||||
#define _SITL_RC_H
|
||||
|
||||
struct RC_ICR4 {
|
||||
uint16_t channels[9]; // 9th channel is sync
|
||||
uint32_t value;
|
||||
uint8_t idx;
|
||||
|
||||
RC_ICR4() {
|
||||
// constructor
|
||||
channels[0] = channels[1] = channels[3] = 1500;
|
||||
channels[4] = channels[7] = 1800;
|
||||
channels[2] = channels[5] = channels[6] = 1000;
|
||||
channels[8] = 4500; // sync
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
read from ICR4 fetches next channel
|
||||
*/
|
||||
operator int() {
|
||||
value += channels[idx++]*2;
|
||||
if (idx == 9) {
|
||||
idx = 0;
|
||||
}
|
||||
value = value % 40000;
|
||||
return (uint16_t)value;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
ignore rate assignment for now (needed for purple
|
||||
emulation)
|
||||
*/
|
||||
RC_ICR4& operator=(uint16_t rate) {
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*
|
||||
interface to set a channel value from SITL
|
||||
*/
|
||||
void set(uint8_t i, uint16_t v) {
|
||||
channels[i] = v;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/*
|
||||
SITL handling - utility functions
|
||||
|
||||
This simulates the APM1 hardware sufficiently for the APM code to
|
||||
think it is running on real hardware
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <fcntl.h>
|
||||
#include "desktop.h"
|
||||
|
||||
|
||||
/*
|
||||
swap one double
|
||||
*/
|
||||
double swap_double(double d)
|
||||
{
|
||||
union {
|
||||
double d;
|
||||
uint8_t b[8];
|
||||
} in, out;
|
||||
int i;
|
||||
in.d = d;
|
||||
for (i=0; i<8; i++) {
|
||||
out.b[7-i] = in.b[i];
|
||||
}
|
||||
return out.d;
|
||||
}
|
||||
|
||||
/*
|
||||
swap an array of doubles
|
||||
*/
|
||||
void swap_doubles(double *d, unsigned count)
|
||||
{
|
||||
while (count--) {
|
||||
*d = swap_double(*d);
|
||||
d++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
swap one float
|
||||
*/
|
||||
float swap_float(float f)
|
||||
{
|
||||
union {
|
||||
float f;
|
||||
uint8_t b[4];
|
||||
} in, out;
|
||||
int i;
|
||||
in.f = f;
|
||||
for (i=0; i<4; i++) {
|
||||
out.b[4-i] = in.b[i];
|
||||
}
|
||||
return out.f;
|
||||
}
|
||||
|
||||
/*
|
||||
swap an array of floats
|
||||
*/
|
||||
void swap_floats(float *f, unsigned count)
|
||||
{
|
||||
while (count--) {
|
||||
*f = swap_float(*f);
|
||||
f++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void set_nonblocking(int fd)
|
||||
{
|
||||
unsigned v = fcntl(fd, F_GETFL, 0);
|
||||
fcntl(fd, F_SETFL, v | O_NONBLOCK);
|
||||
}
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
|
||||
#define ft2m(x) ((x) * 0.3048)
|
||||
#define kt2mps(x) ((x) * 0.514444444)
|
||||
#define sqr(x) ((x)*(x))
|
||||
|
||||
double swap_double(double d);
|
||||
void swap_doubles(double *d, unsigned count);
|
||||
float swap_float(float f);
|
||||
void swap_floats(float *f, unsigned count);
|
||||
void set_nonblocking(int fd);
|
||||
Loading…
Reference in New Issue