From 98ead518016c09079d3d1e13324ac2bc2eff04c7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 15 Dec 2012 12:54:26 +1100 Subject: [PATCH] SITL: fill in a lot more of the AP_HAL SITL backend --- libraries/AP_HAL_AVR_SITL/AnalogIn.cpp | 43 ++ libraries/AP_HAL_AVR_SITL/AnalogIn.h | 41 ++ libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 452 +++++++++++++++++++ libraries/AP_HAL_AVR_SITL/SITL_State.h | 96 ++++ libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp | 57 +++ libraries/AP_HAL_AVR_SITL/sitl_compass.cpp | 80 ++++ libraries/AP_HAL_AVR_SITL/sitl_gps.cpp | 250 ++++++++++ libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 124 +++++ libraries/AP_HAL_AVR_SITL/sitl_rc.h | 52 +++ 9 files changed, 1195 insertions(+) create mode 100644 libraries/AP_HAL_AVR_SITL/AnalogIn.cpp create mode 100644 libraries/AP_HAL_AVR_SITL/AnalogIn.h create mode 100644 libraries/AP_HAL_AVR_SITL/SITL_State.cpp create mode 100644 libraries/AP_HAL_AVR_SITL/SITL_State.h create mode 100644 libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp create mode 100644 libraries/AP_HAL_AVR_SITL/sitl_compass.cpp create mode 100644 libraries/AP_HAL_AVR_SITL/sitl_gps.cpp create mode 100644 libraries/AP_HAL_AVR_SITL/sitl_ins.cpp create mode 100644 libraries/AP_HAL_AVR_SITL/sitl_rc.h diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp new file mode 100644 index 0000000000..fed35b3574 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.cpp @@ -0,0 +1,43 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + +#include "HAL_AVR.h" +#include "AP_HAL_AVR_SITL.h" +#include "AnalogIn.h" +#include + +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 diff --git a/libraries/AP_HAL_AVR_SITL/AnalogIn.h b/libraries/AP_HAL_AVR_SITL/AnalogIn.h new file mode 100644 index 0000000000..32ed746df8 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/AnalogIn.h @@ -0,0 +1,41 @@ + +#ifndef __AP_HAL_AVR_SITL_ANALOG_IN_H__ +#define __AP_HAL_AVR_SITL_ANALOG_IN_H__ + +#include +#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__ diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp new file mode 100644 index 0000000000..98a038e3cb --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -0,0 +1,452 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + +#include +#include +#include "AP_HAL_AVR_SITL_Namespace.h" +#include "HAL_AVR_SITL_Class.h" +#include "UARTDriver.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include +#include + +#include + +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 diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h new file mode 100644 index 0000000000..3492939821 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -0,0 +1,96 @@ + +#ifndef __AP_HAL_AVR_SITL_STATE_H__ +#define __AP_HAL_AVR_SITL_STATE_H__ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + +#include +#include "AP_HAL_AVR_SITL_Namespace.h" +#include "HAL_AVR_SITL_Class.h" +#include "sitl_rc.h" + +#include +#include +#include +#include +#include + +#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__ + diff --git a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp new file mode 100644 index 0000000000..00a5d28e16 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp @@ -0,0 +1,57 @@ +/* + SITL handling + + This simulates a barometer + + Andrew Tridgell November 2011 + */ + +#include +#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 +#include +#include +#include +#include + +/* + 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 diff --git a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp new file mode 100644 index 0000000000..3b8a6c769b --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp @@ -0,0 +1,80 @@ +/* + SITL handling + + This simulates a compass + + Andrew Tridgell November 2011 + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + +#include +#include +#include "AP_HAL_AVR_SITL_Namespace.h" +#include "HAL_AVR_SITL_Class.h" + +#include +#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 diff --git a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp new file mode 100644 index 0000000000..db9596dd50 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp @@ -0,0 +1,250 @@ +/* + SITL handling + + This simulates a GPS on a serial port + + Andrew Tridgell November 2011 + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + +#include +#include +#include "AP_HAL_AVR_SITL_Namespace.h" +#include "HAL_AVR_SITL_Class.h" + +#include +#include "../SITL/SITL.h" +#include "Scheduler.h" +#include "UARTDriver.h" +#include "../AP_GPS/AP_GPS.h" +#include "../AP_GPS/AP_GPS_UBLOX.h" +#include +#include + +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; imillis() - 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; imillis(); // 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 diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp new file mode 100644 index 0000000000..185c99e444 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -0,0 +1,124 @@ +/* + SITL handling + + This emulates the ADS7844 ADC + + Andrew Tridgell November 2011 + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL + +#include +#include +#include "AP_HAL_AVR_SITL_Namespace.h" +#include "HAL_AVR_SITL_Class.h" + +#include +#include "../AP_Compass/AP_Compass.h" +#include "../AP_Declination/AP_Declination.h" +#include "../SITL/SITL.h" +#include "Scheduler.h" +#include +#include "../AP_ADC/AP_ADC.h" +#include + +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 diff --git a/libraries/AP_HAL_AVR_SITL/sitl_rc.h b/libraries/AP_HAL_AVR_SITL/sitl_rc.h new file mode 100644 index 0000000000..efffc0c095 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/sitl_rc.h @@ -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