From 2bed1dcfd509aa1ac7daad49411e9f66d66ac038 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 17 Nov 2011 12:49:56 +1100 Subject: [PATCH] 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 --- ArduCopter/ArduCopter.pde | 5 + ArduPlane/ArduPlane.pde | 6 + libraries/Desktop/Desktop.mk | 2 +- libraries/Desktop/include/avr/iomxx0_1.h | 10 + libraries/Desktop/support/Arduino.cpp | 7 +- libraries/Desktop/support/FastSerial.cpp | 36 ++- libraries/Desktop/support/desktop.h | 18 +- libraries/Desktop/support/main.cpp | 30 +- libraries/Desktop/support/sitl.cpp | 299 +++++++++++++++++++ libraries/Desktop/support/sitl_adc.cpp | 126 ++++++++ libraries/Desktop/support/sitl_adc.h | 63 ++++ libraries/Desktop/support/sitl_barometer.cpp | 36 +++ libraries/Desktop/support/sitl_compass.cpp | 69 +++++ libraries/Desktop/support/sitl_gps.cpp | 159 ++++++++++ libraries/Desktop/support/sitl_rc.h | 52 ++++ libraries/Desktop/support/util.cpp | 81 +++++ libraries/Desktop/support/util.h | 10 + 17 files changed, 971 insertions(+), 38 deletions(-) create mode 100644 libraries/Desktop/support/sitl.cpp create mode 100644 libraries/Desktop/support/sitl_adc.cpp create mode 100644 libraries/Desktop/support/sitl_adc.h create mode 100644 libraries/Desktop/support/sitl_barometer.cpp create mode 100644 libraries/Desktop/support/sitl_compass.cpp create mode 100644 libraries/Desktop/support/sitl_gps.cpp create mode 100644 libraries/Desktop/support/sitl_rc.h create mode 100644 libraries/Desktop/support/util.cpp create mode 100644 libraries/Desktop/support/util.h diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fc760e250b..d225f450a0 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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; diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index ded3b8738b..96e305f8b9 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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 diff --git a/libraries/Desktop/Desktop.mk b/libraries/Desktop/Desktop.mk index 5cf8469c47..2eaa85309d 100644 --- a/libraries/Desktop/Desktop.mk +++ b/libraries/Desktop/Desktop.mk @@ -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. diff --git a/libraries/Desktop/include/avr/iomxx0_1.h b/libraries/Desktop/include/avr/iomxx0_1.h index c1e7d4d194..7b9e6ed3a9 100644 --- a/libraries/Desktop/include/avr/iomxx0_1.h +++ b/libraries/Desktop/include/avr/iomxx0_1.h @@ -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__ */ diff --git a/libraries/Desktop/support/Arduino.cpp b/libraries/Desktop/support/Arduino.cpp index 1726a0e295..355fcc6307 100644 --- a/libraries/Desktop/support/Arduino.cpp +++ b/libraries/Desktop/support/Arduino.cpp @@ -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) diff --git a/libraries/Desktop/support/FastSerial.cpp b/libraries/Desktop/support/FastSerial.cpp index fbb12ff6c7..70b60d9fc0 100644 --- a/libraries/Desktop/support/FastSerial.cpp +++ b/libraries/Desktop/support/FastSerial.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include #include @@ -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; } diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h index aedcdf7d59..a1cf197cb9 100644 --- a/libraries/Desktop/support/desktop.h +++ b/libraries/Desktop/support/desktop.h @@ -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 diff --git a/libraries/Desktop/support/main.cpp b/libraries/Desktop/support/main.cpp index d18d17c712..efec0cb5fc 100644 --- a/libraries/Desktop/support/main.cpp +++ b/libraries/Desktop/support/main.cpp @@ -6,13 +6,14 @@ #include #include #include +#include #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; } diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp new file mode 100644 index 0000000000..ec840bceb0 --- /dev/null +++ b/libraries/Desktop/support/sitl.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 */ ; +} diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp new file mode 100644 index 0000000000..5d2405fc5a --- /dev/null +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -0,0 +1,126 @@ +/* + SITL handling + + This emulates the ADS7844 ADC + + Andrew Tridgell November 2011 + */ +#include +#include +#include +#include +#include +#include +#include +#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); +} diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h new file mode 100644 index 0000000000..68a26fe5c9 --- /dev/null +++ b/libraries/Desktop/support/sitl_adc.h @@ -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 diff --git a/libraries/Desktop/support/sitl_barometer.cpp b/libraries/Desktop/support/sitl_barometer.cpp new file mode 100644 index 0000000000..3bf877737f --- /dev/null +++ b/libraries/Desktop/support/sitl_barometer.cpp @@ -0,0 +1,36 @@ +/* + SITL handling + + This simulates a barometer + + Andrew Tridgell November 2011 + */ +#include +#include +#include +#include +#include +#include // 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); +} diff --git a/libraries/Desktop/support/sitl_compass.cpp b/libraries/Desktop/support/sitl_compass.cpp new file mode 100644 index 0000000000..8ef17bb5f9 --- /dev/null +++ b/libraries/Desktop/support/sitl_compass.cpp @@ -0,0 +1,69 @@ +/* + SITL handling + + This simulates a compass + + Andrew Tridgell November 2011 + */ +#include +#include +#include +#include +#include +#include +#include +#include +#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); +} diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp new file mode 100644 index 0000000000..0148b007ab --- /dev/null +++ b/libraries/Desktop/support/sitl_gps.cpp @@ -0,0 +1,159 @@ +/* + SITL handling + + This simulates a GPS on a serial port + + Andrew Tridgell November 2011 + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 +#include +#include +#include +#include +#include +#include +#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); +} diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h new file mode 100644 index 0000000000..a6ff07ae05 --- /dev/null +++ b/libraries/Desktop/support/util.h @@ -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);