2012-12-14 21:54:26 -04:00
|
|
|
|
|
|
|
#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 <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"
|
2015-01-02 06:50:16 -04:00
|
|
|
#include "../AP_OpticalFlow/AP_OpticalFlow.h"
|
|
|
|
#include "../AP_Terrain/AP_Terrain.h"
|
2012-12-14 21:54:26 -04:00
|
|
|
#include "../SITL/SITL.h"
|
|
|
|
|
|
|
|
class HAL_AVR_SITL;
|
|
|
|
|
|
|
|
class AVR_SITL::SITL_State {
|
2014-11-15 20:04:55 -04:00
|
|
|
friend class AVR_SITL::SITLScheduler;
|
2012-12-14 21:54:26 -04:00
|
|
|
public:
|
|
|
|
void init(int argc, char * const argv[]);
|
|
|
|
|
|
|
|
enum vehicle_type {
|
|
|
|
ArduCopter,
|
|
|
|
APMrover2,
|
|
|
|
ArduPlane
|
|
|
|
};
|
|
|
|
|
2012-12-17 22:34:13 -04:00
|
|
|
int gps_pipe(void);
|
2013-12-21 07:26:30 -04:00
|
|
|
int gps2_pipe(void);
|
2012-12-17 22:34:13 -04:00
|
|
|
ssize_t gps_read(int fd, void *buf, size_t count);
|
2012-12-17 23:56:21 -04:00
|
|
|
static uint16_t pwm_output[11];
|
2013-09-15 20:16:52 -03:00
|
|
|
static uint16_t last_pwm_output[11];
|
2012-12-17 23:56:21 -04:00
|
|
|
static uint16_t pwm_input[8];
|
2014-03-25 00:39:41 -03:00
|
|
|
static bool new_rc_input;
|
2012-12-18 03:12:05 -04:00
|
|
|
static void loop_hook(void);
|
2013-08-14 03:35:08 -03:00
|
|
|
uint16_t base_port(void) const { return _base_port; }
|
2012-12-17 22:34:13 -04:00
|
|
|
|
2013-11-28 06:43:25 -04:00
|
|
|
// simulated airspeed, sonar and battery monitor
|
|
|
|
static uint16_t sonar_pin_value; // pin 0
|
|
|
|
static uint16_t airspeed_pin_value; // pin 1
|
|
|
|
static uint16_t voltage_pin_value; // pin 13
|
|
|
|
static uint16_t current_pin_value; // pin 12
|
2012-12-18 03:42:48 -04:00
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
private:
|
|
|
|
void _parse_command_line(int argc, char * const argv[]);
|
2014-04-10 23:28:32 -03:00
|
|
|
void _set_param_default(char *parm);
|
2012-12-14 21:54:26 -04:00
|
|
|
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
|
2015-01-03 06:47:54 -04:00
|
|
|
static float height_agl(void);
|
2012-12-14 21:54:26 -04:00
|
|
|
static void _update_barometer(float height);
|
2013-05-02 02:01:47 -03:00
|
|
|
static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
|
2015-01-02 06:50:16 -04:00
|
|
|
static void _update_flow(void);
|
2013-02-16 05:16:13 -04:00
|
|
|
|
2013-02-16 07:00:16 -04:00
|
|
|
struct gps_data {
|
|
|
|
double latitude;
|
|
|
|
double longitude;
|
|
|
|
float altitude;
|
|
|
|
double speedN;
|
|
|
|
double speedE;
|
2013-03-27 20:28:44 -03:00
|
|
|
double speedD;
|
2013-02-16 07:00:16 -04:00
|
|
|
bool have_lock;
|
|
|
|
};
|
|
|
|
|
|
|
|
#define MAX_GPS_DELAY 100
|
|
|
|
static gps_data _gps_data[MAX_GPS_DELAY];
|
|
|
|
|
2014-04-04 01:59:04 -03:00
|
|
|
static bool _gps_has_basestation_position;
|
|
|
|
static gps_data _gps_basestation_data;
|
2013-08-13 23:11:46 -03:00
|
|
|
static void _gps_write(const uint8_t *p, uint16_t size);
|
2013-02-16 07:00:16 -04:00
|
|
|
static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
|
|
|
|
static void _update_gps_ubx(const struct gps_data *d);
|
|
|
|
static void _update_gps_mtk(const struct gps_data *d);
|
|
|
|
static void _update_gps_mtk16(const struct gps_data *d);
|
|
|
|
static void _update_gps_mtk19(const struct gps_data *d);
|
2013-08-13 23:11:46 -03:00
|
|
|
static uint16_t _gps_nmea_checksum(const char *s);
|
|
|
|
static void _gps_nmea_printf(const char *fmt, ...);
|
|
|
|
static void _update_gps_nmea(const struct gps_data *d);
|
2014-04-04 01:59:04 -03:00
|
|
|
static void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
|
|
|
|
static void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
|
2013-02-16 07:00:16 -04:00
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
static void _update_gps(double latitude, double longitude, float altitude,
|
2013-03-27 20:28:44 -03:00
|
|
|
double speedN, double speedE, double speedD, bool have_lock);
|
2013-02-16 05:16:13 -04:00
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
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
|
2013-11-26 01:42:43 -04:00
|
|
|
float airspeed, float altitude);
|
2012-12-14 21:54:26 -04:00
|
|
|
static void _fdm_input(void);
|
|
|
|
static void _simulator_output(void);
|
2013-09-15 20:16:52 -03:00
|
|
|
static void _apply_servo_filter(float deltat);
|
2012-12-14 21:54:26 -04:00
|
|
|
static uint16_t _airspeed_sensor(float airspeed);
|
2015-01-03 06:47:54 -04:00
|
|
|
static uint16_t _ground_sonar();
|
2012-12-14 21:54:26 -04:00
|
|
|
static float _gyro_drift(void);
|
|
|
|
static float _rand_float(void);
|
|
|
|
static Vector3f _rand_vec3f(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;
|
2013-08-14 03:35:08 -03:00
|
|
|
static uint16_t _base_port;
|
2012-12-14 21:54:26 -04:00
|
|
|
float _initial_height;
|
|
|
|
static struct sockaddr_in _rcout_addr;
|
|
|
|
static pid_t _parent_pid;
|
|
|
|
static uint32_t _update_count;
|
|
|
|
static bool _motors_on;
|
|
|
|
|
2015-01-05 00:55:50 -04:00
|
|
|
static AP_Baro *_barometer;
|
2014-10-15 02:38:13 -03:00
|
|
|
static AP_InertialSensor *_ins;
|
2012-12-14 21:54:26 -04:00
|
|
|
static SITLScheduler *_scheduler;
|
|
|
|
static AP_Compass_HIL *_compass;
|
2015-01-02 06:50:16 -04:00
|
|
|
static OpticalFlow *_optical_flow;
|
|
|
|
static AP_Terrain *_terrain;
|
2012-12-14 21:54:26 -04:00
|
|
|
|
|
|
|
static int _sitl_fd;
|
|
|
|
static SITL *_sitl;
|
2013-08-14 03:35:08 -03:00
|
|
|
static uint16_t _rcout_port;
|
|
|
|
static uint16_t _simin_port;
|
2014-05-12 06:44:15 -03:00
|
|
|
static float _current;
|
2012-12-14 21:54:26 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
|
|
#endif // __AP_HAL_AVR_SITL_STATE_H__
|
|
|
|
|