Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fill in a lot more of the AP_HAL SITL backend

Browse Source
This commit is contained in:
Andrew Tridgell 2012-12-15 12:54:26 +11:00
parent 4a54ffb523
commit 98ead51801
9 changed files with 1195 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_HAL_AVR_SITL/AnalogIn.cpp Normal file
View File

@ -0,0 +1,43 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include "HAL_AVR.h"
#include "AP_HAL_AVR_SITL.h"
#include "AnalogIn.h"
#include <stdint.h>
using namespace AVR_SITL;
extern const AP_HAL::HAL& hal;
ADCSource::ADCSource(uint8_t pin, float prescale) :
_prescale(prescale)
{}
float ADCSource::read_average() {
return 0;
}
float ADCSource::read_latest() {
return 0;
}
void ADCSource::set_pin(uint8_t pin) {
}
SITLAnalogIn::SITLAnalogIn() {}
void SITLAnalogIn::init(void *ap_hal_scheduler) {
}
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t n) {
return NULL;
}
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t n, float prescale) {
return NULL;
}
#endif

41
libraries/AP_HAL_AVR_SITL/AnalogIn.h Normal file
View File

@ -0,0 +1,41 @@
#ifndef __AP_HAL_AVR_SITL_ANALOG_IN_H__
#define __AP_HAL_AVR_SITL_ANALOG_IN_H__
#include <AP_HAL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#define SITL_INPUT_MAX_CHANNELS 12
class AVR_SITL::ADCSource : public AP_HAL::AnalogSource {
public:
friend class AVR_SITL::SITLAnalogIn;
/* pin designates the ADC input number, or when == AVR_ANALOG_PIN_VCC,
* board vcc */
ADCSource(uint8_t pin, float prescale = 1.0);
/* implement AnalogSource virtual api: */
float read_average();
float read_latest();
void set_pin(uint8_t p);
private:
/* prescale scales the raw measurments for read()*/
const float _prescale;
};
/* AVRAnalogIn : a concrete class providing the implementations of the
* timer event and the AP_HAL::AnalogIn interface */
class AVR_SITL::SITLAnalogIn : public AP_HAL::AnalogIn {
public:
SITLAnalogIn();
void init(void* ap_hal_scheduler);
AP_HAL::AnalogSource* channel(int16_t n);
AP_HAL::AnalogSource* channel(int16_t n, float prescale);
private:
static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS];
};
#endif // __AP_HAL_AVR_SITL_ANALOG_IN_H__

452
libraries/AP_HAL_AVR_SITL/SITL_State.cpp Normal file
View File

@ -0,0 +1,452 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include "UARTDriver.h"
#include "Scheduler.h"
#include <stdio.h>
#include <signal.h>
#include <getopt.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <AP_Param.h>
extern const AP_HAL::HAL& hal;
// emulate RC input
struct RC_ICR4 ICR4;
using namespace AVR_SITL;
enum SITL_State::vehicle_type SITL_State::_vehicle;
uint16_t SITL_State::_framerate;
struct sockaddr_in SITL_State::_rcout_addr;
pid_t SITL_State::_parent_pid;
uint32_t SITL_State::_update_count;
bool SITL_State::_motors_on;
AP_Baro_BMP085_HIL *SITL_State::_barometer;
AP_InertialSensor_Stub *SITL_State::_ins;
SITLScheduler *SITL_State::_scheduler;
AP_Compass_HIL *SITL_State::_compass;
int SITL_State::_sitl_fd;
SITL *SITL_State::_sitl;
uint16_t SITL_State::_pwm_output[11];
// catch floating point exceptions
void SITL_State::_sig_fpe(int signum)
{
printf("ERROR: Floating point exception\n");
exit(1);
}
void SITL_State::_usage(void)
{
printf("Options:\n");
printf("\t-w wipe eeprom and dataflash\n");
printf("\t-r RATE set SITL framerate\n");
printf("\t-H HEIGHT initial barometric height\n");
printf("\t-C use console instead of TCP ports\n");
}
void SITL_State::_parse_command_line(int argc, char * const argv[])
{
int opt;
signal(SIGFPE, _sig_fpe);
while ((opt = getopt(argc, argv, "swhr:H:C")) != -1) {
switch (opt) {
case 'w':
AP_Param::erase_all();
unlink("dataflash.bin");
break;
case 'r':
_framerate = (unsigned)atoi(optarg);
break;
case 'H':
_initial_height = atof(optarg);
break;
case 'C':
AVR_SITL::SITLUARTDriver::_console = true;
break;
default:
_usage();
exit(1);
}
}
printf("Starting sketch '%s'\n", SKETCH);
if (strcmp(SKETCH, "ArduCopter") == 0) {
_vehicle = ArduCopter;
if (_framerate == 0) {
_framerate = 200;
}
} else if (strcmp(SKETCH, "APMrover2") == 0) {
_vehicle = APMrover2;
if (_framerate == 0) {
_framerate = 50;
}
// set right default throttle for rover (allowing for reverse)
ICR4.set(2, 1500);
} else {
_vehicle = ArduPlane;
if (_framerate == 0) {
_framerate = 50;
}
}
_sitl_setup();
}
/*
setup for SITL handling
*/
void SITL_State::_sitl_setup(void)
{
#ifndef __CYGWIN__
_parent_pid = getppid();
#endif
_rcout_addr.sin_family = AF_INET;
_rcout_addr.sin_port = htons(_rcout_port);
inet_pton(AF_INET, "127.0.0.1", &_rcout_addr.sin_addr);
_setup_timer();
_setup_fdm();
printf("Starting SITL input\n");
// find the barometer object if it exists
_sitl = (SITL *)AP_Param::find_object("SIM_");
_barometer = (AP_Baro_BMP085_HIL *)AP_Param::find_object("GND_");
_ins = (AP_InertialSensor_Stub *)AP_Param::find_object("INS_");
_compass = (AP_Compass_HIL *)AP_Param::find_object("COMPASS_");
if (_sitl != NULL) {
// setup some initial values
_update_barometer(_initial_height);
_update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0);
_update_compass(0, 0, 0);
_update_gps(0, 0, 0, 0, 0, false);
}
}
/*
setup a SITL FDM listening UDP port
*/
void SITL_State::_setup_fdm(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(_simin_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);
}
AVR_SITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
}
/*
timer called at 1kHz
*/
void SITL_State::_timer_handler(int signum)
{
static uint32_t last_update_count;
static bool in_timer;
if (in_timer || ((SITLScheduler *)hal.scheduler)->interrupts_are_blocked()) {
return;
}
_scheduler->begin_atomic();
in_timer = true;
#ifndef __CYGWIN__
/* make sure we die if our parent dies */
if (kill(_parent_pid, 0) != 0) {
exit(1);
}
#else
static uint16_t count = 0;
static uint32_t last_report;
count++;
if (millis() - last_report > 1000) {
printf("TH %u cps\n", count);
count = 0;
last_report = millis();
}
#endif
/* check for packet from flight sim */
_fdm_input();
// send RC output to flight sim
_simulator_output();
if (_update_count == 0 && _sitl != NULL) {
_update_gps(0, 0, 0, 0, 0, false);
_scheduler->timer_event();
_scheduler->end_atomic();
in_timer = false;
return;
}
if (_update_count == last_update_count) {
_scheduler->timer_event();
_scheduler->end_atomic();
in_timer = false;
return;
}
last_update_count = _update_count;
if (_sitl != NULL) {
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
_sitl->state.altitude,
_sitl->state.speedN, _sitl->state.speedE, !_sitl->gps_disable);
_update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
_sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
_sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
_sitl->state.airspeed);
_update_barometer(_sitl->state.altitude);
_update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.heading);
}
// trigger all APM timers. We do this last as it can re-enable
// interrupts, which can lead to recursion
_scheduler->timer_event();
_scheduler->end_atomic();
in_timer = false;
}
/*
check for a SITL FDM packet
*/
void SITL_State::_fdm_input(void)
{
ssize_t size;
struct pwm_packet {
uint16_t pwm[8];
};
union {
struct sitl_fdm 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;
if (d.fg_pkt.magic != 0x4c56414e) {
printf("Bad FDM 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->state = d.fg_pkt;
_update_count++;
count++;
if (hal.scheduler->millis() - last_report > 1000) {
//printf("SIM %u FPS\n", count);
count = 0;
last_report = hal.scheduler->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
*/
void SITL_State::_simulator_output(void)
{
static uint32_t last_update;
struct {
uint16_t pwm[11];
uint16_t speed, direction, turbulance;
} control;
/* this maps the registers used for PWM outputs. The RC
* driver updates these whenever it wants the channel output
* to change */
uint8_t i;
if (last_update == 0) {
for (i=0; i<11; i++) {
_pwm_output[i] = 1000;
}
if (_vehicle == ArduPlane) {
_pwm_output[0] = _pwm_output[1] = _pwm_output[3] = 1500;
_pwm_output[7] = 1800;
}
if (_vehicle == APMrover2) {
_pwm_output[0] = _pwm_output[1] = _pwm_output[2] = _pwm_output[3] = 1500;
_pwm_output[7] = 1800;
}
}
if (_sitl == NULL) {
return;
}
// output at chosen framerate
if (last_update != 0 && hal.scheduler->millis() - last_update < 1000/_framerate) {
return;
}
last_update = hal.scheduler->millis();
for (i=0; i<11; i++) {
if (_pwm_output[i] == 0xFFFF) {
control.pwm[i] = 0;
} else {
control.pwm[i] = _pwm_output[i];
}
}
if (_vehicle == ArduPlane) {
// add in engine multiplier
if (control.pwm[2] > 1000) {
control.pwm[2] = ((control.pwm[2]-1000) * _sitl->engine_mul) + 1000;
if (control.pwm[2] > 2000) control.pwm[2] = 2000;
}
_motors_on = ((control.pwm[2]-1000)/1000.0) > 0;
} else if (_vehicle == APMrover2) {
// add in engine multiplier
if (control.pwm[2] != 1500) {
control.pwm[2] = ((control.pwm[2]-1500) * _sitl->engine_mul) + 1500;
if (control.pwm[2] > 2000) control.pwm[2] = 2000;
if (control.pwm[2] < 1000) control.pwm[2] = 1000;
}
_motors_on = ((control.pwm[2]-1500)/500.0) != 0;
} else {
_motors_on = false;
for (i=0; i<4; i++) {
if ((control.pwm[i]-1000)/1000.0 > 0) {
_motors_on = true;
}
}
}
// setup wind control
control.speed = _sitl->wind_speed * 100;
float direction = _sitl->wind_direction;
if (direction < 0) {
direction += 360;
}
control.direction = direction * 100;
control.turbulance = _sitl->wind_turbulance * 100;
// zero the wind for the first 15s to allow pitot calibration
if (hal.scheduler->millis() < 15000) {
control.speed = 0;
}
sendto(_sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&_rcout_addr, sizeof(_rcout_addr));
}
/*
setup a timer used to prod the ISRs
*/
void SITL_State::_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);
}
// generate a random float between -1 and 1
float SITL_State::_rand_float(void)
{
return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
}
// generate a random Vector3f of size 1
Vector3f SITL_State::_rand_vec3f(void)
{
Vector3f v = Vector3f(_rand_float(),
_rand_float(),
_rand_float());
if (v.length() != 0.0) {
v.normalize();
}
return v;
}
void SITL_State::init(int argc, char * const argv[])
{
_scheduler = (SITLScheduler *)hal.scheduler;
_parse_command_line(argc, argv);
}
#endif

96
libraries/AP_HAL_AVR_SITL/SITL_State.h Normal file
View File

@ -0,0 +1,96 @@
#ifndef __AP_HAL_AVR_SITL_STATE_H__
#define __AP_HAL_AVR_SITL_STATE_H__
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include "sitl_rc.h"
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/udp.h>
#include <arpa/inet.h>
#include "../AP_Baro/AP_Baro.h"
#include "../AP_InertialSensor/AP_InertialSensor.h"
#include "../AP_Compass/AP_Compass.h"
#include "../SITL/SITL.h"
extern struct RC_ICR4 ICR4;
class HAL_AVR_SITL;
class AVR_SITL::SITL_State {
public:
void init(int argc, char * const argv[]);
enum vehicle_type {
ArduCopter,
APMrover2,
ArduPlane
};
private:
void _parse_command_line(int argc, char * const argv[]);
void _usage(void);
void _sitl_setup(void);
void _setup_fdm(void);
void _setup_timer(void);
void _setup_adc(void);
// these methods are static as they are called
// from the timer
static void _update_barometer(float height);
static void _update_compass(float roll, float pitch, float yaw);
static void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock);
static void _update_ins(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 void _fdm_input(void);
static void _simulator_output(void);
static uint16_t _airspeed_sensor(float airspeed);
static float _gyro_drift(void);
static float _rand_float(void);
static Vector3f _rand_vec3f(void);
static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
static void _gps_send(uint8_t msgid, uint8_t *buf, uint16_t size);
ssize_t _gps_read(int fd, void *buf, size_t count);
int _gps_pipe(void);
// signal handlers
static void _sig_fpe(int signum);
static void _timer_handler(int signum);
// internal state
static enum vehicle_type _vehicle;
static uint16_t _framerate;
float _initial_height;
static struct sockaddr_in _rcout_addr;
static pid_t _parent_pid;
static uint32_t _update_count;
static bool _motors_on;
static AP_Baro_BMP085_HIL *_barometer;
static AP_InertialSensor_Stub *_ins;
static SITLScheduler *_scheduler;
static AP_Compass_HIL *_compass;
static int _sitl_fd;
static SITL *_sitl;
static const uint16_t _rcout_port = 5502;
static const uint16_t _simin_port = 5501;
static uint16_t _pwm_output[11];
};
#endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#endif // __AP_HAL_AVR_SITL_STATE_H__

57
libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp Normal file
View File

@ -0,0 +1,57 @@
/*
SITL handling
This simulates a barometer
Andrew Tridgell November 2011
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include "HAL_AVR.h"
#include "AP_HAL_AVR_SITL.h"
using namespace AVR_SITL;
extern const AP_HAL::HAL& hal;
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
/*
setup the barometer with new input
altitude is in meters
*/
void SITL_State::_update_barometer(float altitude)
{
double Temp, Press, y;
static uint32_t last_update;
if (_barometer == NULL) {
// this sketch doesn't use a barometer
return;
}
// 80Hz, to match the real APM2 barometer
if (hal.scheduler->millis() - last_update < 12) {
return;
}
last_update = hal.scheduler->millis();
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 + (_rand_float() * _sitl->baro_noise);
_barometer->setHIL(Temp, Press);
}
#endif

80
libraries/AP_HAL_AVR_SITL/sitl_compass.cpp Normal file
View File

@ -0,0 +1,80 @@
/*
SITL handling
This simulates a compass
Andrew Tridgell November 2011
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include <AP_Math.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_Declination/AP_Declination.h"
#include "../SITL/SITL.h"
using namespace AVR_SITL;
#define MAG_OFS_X 5.0
#define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0
// inclination in Canberra (degrees)
#define MAG_INCLINATION -66
// magnetic field strength in Canberra as observed
// using an APM1 with 5883L compass
#define MAG_FIELD_STRENGTH 818
/*
given a magnetic heading, and roll, pitch, yaw values,
calculate consistent magnetometer components
All angles are in radians
*/
Vector3f SITL_State::_heading_to_mag(float roll, float pitch, float yaw)
{
Vector3f Bearth, m;
Matrix3f R;
float declination = AP_Declination::get_declination(_sitl->state.latitude, _sitl->state.longitude);
// Bearth is the magnetic field in Canberra. We need to adjust
// it for inclination and declination
Bearth(MAG_FIELD_STRENGTH, 0, 0);
R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(declination));
Bearth = R * Bearth;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
return m + (_rand_vec3f() * _sitl->mag_noise);
}
/*
setup the compass with new input
all inputs are in degrees
*/
void SITL_State::_update_compass(float roll, float pitch, float yaw)
{
if (_compass == NULL) {
// no compass in this sketch
return;
}
Vector3f m = _heading_to_mag(ToRad(roll),
ToRad(pitch),
ToRad(yaw));
_compass->setHIL(m.x, m.y, m.z);
}
#endif

250
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp Normal file
View File

@ -0,0 +1,250 @@
/*
SITL handling
This simulates a GPS on a serial port
Andrew Tridgell November 2011
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include <AP_Math.h>
#include "../SITL/SITL.h"
#include "Scheduler.h"
#include "UARTDriver.h"
#include "../AP_GPS/AP_GPS.h"
#include "../AP_GPS/AP_GPS_UBLOX.h"
#include <sys/ioctl.h>
#include <unistd.h>
using namespace AVR_SITL;
extern const AP_HAL::HAL& hal;
#define MAX_GPS_DELAY 100
struct gps_data {
double latitude;
double longitude;
float altitude;
double speedN;
double speedE;
bool have_lock;
} gps_data[MAX_GPS_DELAY];
static uint8_t next_gps_index;
static uint8_t gps_delay;
// state of GPS emulation
static struct {
/* pipe emulating UBLOX GPS serial stream */
int gps_fd, client_fd;
uint32_t last_update; // milliseconds
} gps_state;
/*
hook for reading from the GPS pipe
*/
ssize_t SITL_State::_gps_read(int fd, void *buf, size_t count)
{
#ifdef FIONREAD
// use FIONREAD to get exact value if possible
int num_ready;
while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 256) {
// the pipe is filling up - drain it
uint8_t tmp[128];
if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) {
break;
}
}
#endif
return read(fd, buf, count);
}
/*
setup GPS input pipe
*/
int SITL_State::_gps_pipe(void)
{
int fd[2];
if (gps_state.client_fd != 0) {
return gps_state.client_fd;
}
pipe(fd);
gps_state.gps_fd = fd[1];
gps_state.client_fd = fd[0];
gps_state.last_update = _scheduler->millis();
AVR_SITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd);
AVR_SITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps_state.client_fd;
}
/*
send a UBLOX GPS message
*/
void SITL_State::_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_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock)
{
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;
struct ubx_nav_solution {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} sol;
const uint8_t MSG_POSLLH = 0x2;
const uint8_t MSG_STATUS = 0x3;
const uint8_t MSG_VELNED = 0x12;
const uint8_t MSG_SOL = 0x6;
struct gps_data d;
// 5Hz, to match the real UBlox config in APM
if (hal.scheduler->millis() - gps_state.last_update < 200) {
return;
}
gps_state.last_update = hal.scheduler->millis();
d.latitude = latitude;
d.longitude = longitude;
d.altitude = altitude;
d.speedN = speedN;
d.speedE = speedE;
d.have_lock = have_lock;
// add in some GPS lag
gps_data[next_gps_index++] = d;
if (next_gps_index >= gps_delay) {
next_gps_index = 0;
}
d = gps_data[next_gps_index];
if (_sitl->gps_delay != gps_delay) {
// cope with updates to the delay control
gps_delay = _sitl->gps_delay;
for (uint8_t i=0; i<gps_delay; i++) {
gps_data[i] = d;
}
}
pos.time = hal.scheduler->millis(); // FIX
pos.longitude = d.longitude * 1.0e7;
pos.latitude = d.latitude * 1.0e7;
pos.altitude_ellipsoid = d.altitude*1000.0;
pos.altitude_msl = d.altitude*1000.0;
pos.horizontal_accuracy = 5;
pos.vertical_accuracy = 10;
status.time = pos.time;
status.fix_type = d.have_lock?3:0;
status.fix_status = d.have_lock?1:0;
status.differential_status = 0;
status.res = 0;
status.time_to_first_fix = 0;
status.uptime = hal.scheduler->millis();
velned.time = pos.time;
velned.ned_north = 100.0 * d.speedN;
velned.ned_east = 100.0 * d.speedE;
velned.ned_down = 0;
#define sqr(x) ((x)*(x))
velned.speed_2d = sqrt(sqr(d.speedN)+sqr(d.speedE)) * 100;
velned.speed_3d = velned.speed_2d;
velned.heading_2d = ToDeg(atan2(d.speedE, d.speedN)) * 100000.0;
if (velned.heading_2d < 0.0) {
velned.heading_2d += 360.0 * 100000.0;
}
velned.speed_accuracy = 2;
velned.heading_accuracy = 4;
memset(&sol, 0, sizeof(sol));
sol.fix_type = d.have_lock?3:0;
sol.fix_status = 221;
sol.satellites = d.have_lock?10:3;
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));
_gps_send(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
}
#endif

124
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp Normal file
View File

@ -0,0 +1,124 @@
/*
SITL handling
This emulates the ADS7844 ADC
Andrew Tridgell November 2011
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include <AP_Math.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_Declination/AP_Declination.h"
#include "../SITL/SITL.h"
#include "Scheduler.h"
#include <AP_Math.h>
#include "../AP_ADC/AP_ADC.h"
#include <SITL_State.h>
using namespace AVR_SITL;
/*
convert airspeed in m/s to an airspeed sensor value
*/
uint16_t SITL_State::_airspeed_sensor(float airspeed)
{
const float airspeed_ratio = 1.9936;
const float airspeed_offset = 2820;
float airspeed_pressure, airspeed_raw;
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
airspeed_raw = airspeed_pressure + airspeed_offset;
return airspeed_raw;
}
float SITL_State::_gyro_drift(void)
{
if (_sitl->drift_speed == 0.0) {
return 0;
}
double period = _sitl->drift_time * 2;
double minutes = fmod(_scheduler->_micros() / 60.0e6, period);
if (minutes < period/2) {
return minutes * ToRad(_sitl->drift_speed);
}
return (period - minutes) * ToRad(_sitl->drift_speed);
}
/*
setup the INS input 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_State::_update_ins(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)
{
double p, q, r;
if (_ins == NULL) {
// no inertial sensor in this sketch
return;
}
SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);
// minimum noise levels are 2 bits
float accel_noise = 0.1;
float gyro_noise = ToRad(0.4);
if (_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();
_ins->set_gyro(Vector3f(p, q, r));
_ins->set_accel(Vector3f(xAccel, yAccel, zAccel));
}
#endif

52
libraries/AP_HAL_AVR_SITL/sitl_rc.h Normal file
View File

@ -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 apm2
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