Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: reformat HAL_SITL with astyle

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-05 10:59:07 +10:00
parent 8c8c910ad0
commit c81ad1d622
23 changed files with 889 additions and 868 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
View File

@ -3,16 +3,16 @@
#define __AP_HAL_SITL_NAMESPACE_H__
namespace HALSITL {
class SITLUARTDriver;
class SITLScheduler;
class SITL_State;
class SITLEEPROMStorage;
class SITLAnalogIn;
class SITLRCInput;
class SITLRCOutput;
class ADCSource;
class RCInput;
class SITLUtil;
class SITLUARTDriver;
class SITLScheduler;
class SITL_State;
class SITLEEPROMStorage;
class SITLAnalogIn;
class SITLRCInput;
class SITLRCOutput;
class ADCSource;
class RCInput;
class SITLUtil;
}
#endif // __AP_HAL_SITL_NAMESPACE_H__

6
libraries/AP_HAL_SITL/AnalogIn.cpp
View File

@ -17,15 +17,15 @@ ADCSource::ADCSource(SITL_State *sitlState, uint8_t pin) :
{}
float ADCSource::read_average() {
return read_latest();
return read_latest();
}
float ADCSource::voltage_average() {
return (5.0f/1023.0f) * read_average();
return (5.0f/1023.0f) * read_average();
}
float ADCSource::voltage_latest() {
return (5.0f/1023.0f) * read_latest();
return (5.0f/1023.0f) * read_latest();
}
float ADCSource::read_latest() {

10
libraries/AP_HAL_SITL/AnalogIn.h
View File

@ -19,7 +19,9 @@ public:
void set_pin(uint8_t p);
float voltage_average();
float voltage_latest();
float voltage_average_ratiometric() { return voltage_average(); }
float voltage_average_ratiometric() {
return voltage_average();
}
void set_stop_pin(uint8_t pin) {}
void set_settle_time(uint16_t settle_time_ms) {}
@ -33,11 +35,13 @@ private:
class HALSITL::SITLAnalogIn : public AP_HAL::AnalogIn {
public:
SITLAnalogIn(SITL_State *sitlState) {
_sitlState = sitlState;
_sitlState = sitlState;
}
void init(void* ap_hal_scheduler);
AP_HAL::AnalogSource* channel(int16_t n);
float board_voltage(void) { return 5.0f; }
float board_voltage(void) {
return 5.0f;
}
private:
static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS];
SITL_State *_sitlState;

2
libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
View File

@ -44,7 +44,7 @@ static SITLUtil utilInstance;
HAL_SITL::HAL_SITL() :
AP_HAL::HAL(
&sitlUart0Driver, /* uartA */
&sitlUart0Driver, /* uartA */
&sitlUart1Driver, /* uartB */
&sitlUart2Driver, /* uartC */
&sitlUart3Driver, /* uartD */

4
libraries/AP_HAL_SITL/RCInput.cpp
View File

@ -30,7 +30,7 @@ uint16_t SITLRCInput::read(uint8_t ch) {
uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) {
for (uint8_t i=0; i<len; i++) {
periods[i] = _override[i]? _override[i] : _sitlState->pwm_input[i];
periods[i] = _override[i]? _override[i] : _sitlState->pwm_input[i];
}
return 8;
}
@ -57,7 +57,7 @@ bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
void SITLRCInput::clear_overrides()
{
for (uint8_t i = 0; i < 8; i++) {
_override[i] = 0;
_override[i] = 0;
}
}
#endif

6
libraries/AP_HAL_SITL/RCInput.h
View File

@ -9,11 +9,13 @@
class HALSITL::SITLRCInput : public AP_HAL::RCInput {
public:
SITLRCInput(SITL_State *sitlState) {
_sitlState = sitlState;
_sitlState = sitlState;
}
void init(void* machtnichts);
bool new_input();
uint8_t num_channels() { return 8; }
uint8_t num_channels() {
return 8;
}
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);

8
libraries/AP_HAL_SITL/RCOutput.cpp
View File

@ -24,26 +24,26 @@ void SITLRCOutput::disable_ch(uint8_t ch)
void SITLRCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch < 11) {
_sitlState->pwm_output[ch] = period_us;
_sitlState->pwm_output[ch] = period_us;
}
}
void SITLRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
{
memcpy(_sitlState->pwm_output+ch, period_us, len*sizeof(uint16_t));
memcpy(_sitlState->pwm_output+ch, period_us, len*sizeof(uint16_t));
}
uint16_t SITLRCOutput::read(uint8_t ch)
{
if (ch < 11) {
return _sitlState->pwm_output[ch];
return _sitlState->pwm_output[ch];
}
return 0;
}
void SITLRCOutput::read(uint16_t* period_us, uint8_t len)
{
memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
}
#endif

4
libraries/AP_HAL_SITL/RCOutput.h
View File

@ -9,8 +9,8 @@
class HALSITL::SITLRCOutput : public AP_HAL::RCOutput {
public:
SITLRCOutput(SITL_State *sitlState) {
_sitlState = sitlState;
_freq_hz = 50;
_sitlState = sitlState;
_freq_hz = 50;
}
void init(void* machtnichts);
void set_freq(uint32_t chmask, uint16_t freq_hz);

302
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -62,24 +62,24 @@ void SITL_State::_set_param_default(const char *parm)
void SITL_State::_sitl_setup(void)
{
#ifndef __CYGWIN__
_parent_pid = getppid();
_parent_pid = getppid();
#endif
_rcout_addr.sin_family = AF_INET;
_rcout_addr.sin_port = htons(_rcout_port);
inet_pton(AF_INET, _fdm_address, &_rcout_addr.sin_addr);
_rcout_addr.sin_family = AF_INET;
_rcout_addr.sin_port = htons(_rcout_port);
inet_pton(AF_INET, _fdm_address, &_rcout_addr.sin_addr);
#ifndef HIL_MODE
_setup_fdm();
_setup_fdm();
#endif
fprintf(stdout, "Starting SITL input\n");
fprintf(stdout, "Starting SITL input\n");
// find the barometer object if it exists
_sitl = (SITL *)AP_Param::find_object("SIM_");
_barometer = (AP_Baro *)AP_Param::find_object("GND_");
_ins = (AP_InertialSensor *)AP_Param::find_object("INS_");
_compass = (Compass *)AP_Param::find_object("COMPASS_");
_terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
_optical_flow = (OpticalFlow *)AP_Param::find_object("FLOW");
// find the barometer object if it exists
_sitl = (SITL *)AP_Param::find_object("SIM_");
_barometer = (AP_Baro *)AP_Param::find_object("GND_");
_ins = (AP_InertialSensor *)AP_Param::find_object("INS_");
_compass = (Compass *)AP_Param::find_object("COMPASS_");
_terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
_optical_flow = (OpticalFlow *)AP_Param::find_object("FLOW");
if (_sitl != NULL) {
// setup some initial values
@ -104,34 +104,34 @@ void SITL_State::_sitl_setup(void)
*/
void SITL_State::_setup_fdm(void)
{
int one=1, ret;
struct sockaddr_in sockaddr;
int one=1, ret;
struct sockaddr_in sockaddr;
memset(&sockaddr,0,sizeof(sockaddr));
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(_simin_port);
sockaddr.sin_family = AF_INET;
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);
}
_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));
/* 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);
}
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);
}
HALSITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
}
#endif
@ -221,37 +221,37 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
*/
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;
ssize_t size;
struct pwm_packet {
uint16_t pwm[8];
};
union {
struct sitl_fdm fg_pkt;
struct pwm_packet pwm_pkt;
} d;
bool got_fg_input = false;
size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
switch (size) {
size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
switch (size) {
case 148:
static uint32_t last_report;
static uint32_t count;
static uint32_t last_report;
static uint32_t count;
if (d.fg_pkt.magic != 0x4c56414f) {
fprintf(stdout, "Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic);
return;
}
if (d.fg_pkt.magic != 0x4c56414f) {
fprintf(stdout, "Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic);
return;
}
hal.scheduler->stop_clock(d.fg_pkt.timestamp_us);
_synthetic_clock_mode = true;
got_fg_input = true;
if (d.fg_pkt.latitude == 0 ||
d.fg_pkt.longitude == 0 ||
d.fg_pkt.altitude <= 0) {
// garbage input
return;
}
if (d.fg_pkt.latitude == 0 ||
d.fg_pkt.longitude == 0 ||
d.fg_pkt.altitude <= 0) {
// garbage input
return;
}
if (_sitl != NULL) {
_sitl->state = d.fg_pkt;
@ -263,28 +263,28 @@ void SITL_State::_fdm_input(void)
}
}
}
_update_count++;
_update_count++;
count++;
if (hal.scheduler->millis() - last_report > 1000) {
//fprintf(stdout, "SIM %u FPS\n", count);
count = 0;
last_report = hal.scheduler->millis();
}
break;
count++;
if (hal.scheduler->millis() - last_report > 1000) {
//fprintf(stdout, "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 pwn input for the RC channel inputs
if (d.pwm_pkt.pwm[i] != 0) {
pwm_input[i] = d.pwm_pkt.pwm[i];
}
}
break;
}
}
case 16: {
// a packet giving the receiver PWM inputs
uint8_t i;
for (i=0; i<8; i++) {
// setup the pwn input for the RC channel inputs
if (d.pwm_pkt.pwm[i] != 0) {
pwm_input[i] = d.pwm_pkt.pwm[i];
}
}
break;
}
}
if (got_fg_input) {
// send RC output to flight sim
@ -353,74 +353,74 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
{
static uint32_t last_update_usec;
/* this maps the registers used for PWM outputs. The RC
* driver updates these whenever it wants the channel output
* to change */
uint8_t i;
/* 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_usec == 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;
}
for (i=0; i<11; i++) {
if (last_update_usec == 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;
}
for (i=0; i<11; i++) {
last_pwm_output[i] = pwm_output[i];
}
}
}
// output at chosen framerate
// output at chosen framerate
uint32_t now = hal.scheduler->micros();
float deltat = (now - last_update_usec) * 1.0e-6f;
last_update_usec = now;
last_update_usec = now;
_apply_servo_filter(deltat);
for (i=0; i<11; i++) {
if (pwm_output[i] == 0xFFFF) {
input.servos[i] = 0;
} else {
input.servos[i] = pwm_output[i];
}
for (i=0; i<11; i++) {
if (pwm_output[i] == 0xFFFF) {
input.servos[i] = 0;
} else {
input.servos[i] = pwm_output[i];
}
last_pwm_output[i] = pwm_output[i];
}
}
if (_vehicle == ArduPlane) {
// add in engine multiplier
if (input.servos[2] > 1000) {
input.servos[2] = ((input.servos[2]-1000) * _sitl->engine_mul) + 1000;
if (input.servos[2] > 2000) input.servos[2] = 2000;
}
_motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
} else if (_vehicle == APMrover2) {
// add in engine multiplier
if (input.servos[2] != 1500) {
input.servos[2] = ((input.servos[2]-1500) * _sitl->engine_mul) + 1500;
if (input.servos[2] > 2000) input.servos[2] = 2000;
if (input.servos[2] < 1000) input.servos[2] = 1000;
}
_motors_on = ((input.servos[2]-1500)/500.0f) != 0;
} else {
_motors_on = false;
if (_vehicle == ArduPlane) {
// add in engine multiplier
if (input.servos[2] > 1000) {
input.servos[2] = ((input.servos[2]-1000) * _sitl->engine_mul) + 1000;
if (input.servos[2] > 2000) input.servos[2] = 2000;
}
_motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
} else if (_vehicle == APMrover2) {
// add in engine multiplier
if (input.servos[2] != 1500) {
input.servos[2] = ((input.servos[2]-1500) * _sitl->engine_mul) + 1500;
if (input.servos[2] > 2000) input.servos[2] = 2000;
if (input.servos[2] < 1000) input.servos[2] = 1000;
}
_motors_on = ((input.servos[2]-1500)/500.0f) != 0;
} else {
_motors_on = false;
// apply engine multiplier to first motor
input.servos[0] = ((input.servos[0]-1000) * _sitl->engine_mul) + 1000;
// run checks on each motor
for (i=0; i<4; i++) {
for (i=0; i<4; i++) {
// check motors do not exceed their limits
if (input.servos[i] > 2000) input.servos[i] = 2000;
if (input.servos[i] < 1000) input.servos[i] = 1000;
// update motor_on flag
if ((input.servos[i]-1000)/1000.0f > 0) {
if ((input.servos[i]-1000)/1000.0f > 0) {
_motors_on = true;
}
}
}
}
}
}
float throttle = _motors_on?(input.servos[2]-1000) / 1000.0f:0;
// lose 0.7V at full throttle
@ -440,10 +440,10 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
*/
void SITL_State::_simulator_output(bool synthetic_clock_mode)
{
struct {
uint16_t pwm[11];
uint16_t speed, direction, turbulance;
} control;
struct {
uint16_t pwm[11];
uint16_t speed, direction, turbulance;
} control;
Aircraft::sitl_input input;
_simulator_servos(input);
@ -454,7 +454,7 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode)
memcpy(control.pwm, input.servos, sizeof(control.pwm));
// setup wind control
// setup wind control
float wind_speed = _sitl->wind_speed * 100;
float altitude = _barometer?_barometer->get_altitude():0;
if (altitude < 0) {
@ -463,20 +463,20 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode)
if (altitude < 60) {
wind_speed *= altitude / 60.0f;
}
control.speed = wind_speed;
float direction = _sitl->wind_direction;
if (direction < 0) {
direction += 360;
}
control.direction = direction * 100;
control.turbulance = _sitl->wind_turbulance * 100;
control.speed = wind_speed;
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;
}
// 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));
sendto(_sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&_rcout_addr, sizeof(_rcout_addr));
}
// generate a random float between -1 and 1
@ -488,13 +488,13 @@ float SITL_State::_rand_float(void)
// generate a random Vector3f of size 1
Vector3f SITL_State::_rand_vec3f(void)
{
Vector3f v = Vector3f(_rand_float(),
Vector3f v = Vector3f(_rand_float(),
_rand_float(),
_rand_float());
if (v.length() != 0.0f) {
v.normalize();
}
return v;
if (v.length() != 0.0f) {
v.normalize();
}
return v;
}
@ -505,7 +505,7 @@ void SITL_State::init(int argc, char * const argv[])
pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
_scheduler = (SITLScheduler *)hal.scheduler;
_parse_command_line(argc, argv);
_parse_command_line(argc, argv);
}
/*
@ -513,15 +513,15 @@ void SITL_State::init(int argc, char * const argv[])
*/
float SITL_State::height_agl(void)
{
static float home_alt = -1;
static float home_alt = -1;
if (home_alt == -1 && _sitl->state.altitude > 0) {
if (home_alt == -1 && _sitl->state.altitude > 0) {
// remember home altitude as first non-zero altitude
home_alt = _sitl->state.altitude;
home_alt = _sitl->state.altitude;
}
if (_terrain &&
_sitl->terrain_enable) {
_sitl->terrain_enable) {
// get height above terrain from AP_Terrain. This assumes
// AP_Terrain is working
float terrain_height_amsl;

34
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -32,9 +32,9 @@ public:
void init(int argc, char * const argv[]);
enum vehicle_type {
ArduCopter,
APMrover2,
ArduPlane
ArduCopter,
APMrover2,
ArduPlane
};
int gps_pipe(void);
@ -45,7 +45,9 @@ public:
uint16_t pwm_input[8];
bool new_rc_input;
void loop_hook(void);
uint16_t base_port(void) const { return _base_port; }
uint16_t base_port(void) const {
return _base_port;
}
// simulated airspeed, sonar and battery monitor
uint16_t sonar_pin_value; // pin 0
@ -68,16 +70,16 @@ private:
void _update_flow(void);
struct gps_data {
double latitude;
double longitude;
float altitude;
double speedN;
double speedE;
double speedD;
bool have_lock;
double latitude;
double longitude;
float altitude;
double speedN;
double speedE;
double speedD;
bool have_lock;
};
#define MAX_GPS_DELAY 100
#define MAX_GPS_DELAY 100
gps_data _gps_data[MAX_GPS_DELAY];
bool _gps_has_basestation_position;
@ -95,12 +97,12 @@ private:
void _update_gps_sbp(const struct gps_data *d);
void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock);
double speedN, double speedE, double speedD, bool have_lock);
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, float altitude);
double rollRate, double pitchRate,double yawRate, // Local to plane
double xAccel, double yAccel, double zAccel, // Local to plane
float airspeed, float altitude);
void _fdm_input(void);
void _fdm_input_local(void);
void _simulator_servos(Aircraft::sitl_input &input);

124
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -24,22 +24,22 @@ using namespace HALSITL;
// catch floating point exceptions
static void _sig_fpe(int signum)
{
fprintf(stderr, "ERROR: Floating point exception - aborting\n");
fprintf(stderr, "ERROR: Floating point exception - aborting\n");
abort();
}
void SITL_State::_usage(void)
{
fprintf(stdout, "Options:\n");
fprintf(stdout, "\t-w wipe eeprom and dataflash\n");
fprintf(stdout, "\t-r RATE set SITL framerate\n");
fprintf(stdout, "\t-H HEIGHT initial barometric height\n");
fprintf(stdout, "\t-C use console instead of TCP ports\n");
fprintf(stdout, "\t-I set instance of SITL (adds 10*instance to all port numbers)\n");
fprintf(stdout, "\t-s SPEEDUP simulation speedup\n");
fprintf(stdout, "\t-O ORIGIN set home location (lat,lng,alt,yaw)\n");
fprintf(stdout, "\t-M MODEL set simulation model\n");
fprintf(stdout, "\t-F FDMADDR set FDM UDP address (IPv4)\n");
fprintf(stdout, "Options:\n");
fprintf(stdout, "\t-w wipe eeprom and dataflash\n");
fprintf(stdout, "\t-r RATE set SITL framerate\n");
fprintf(stdout, "\t-H HEIGHT initial barometric height\n");
fprintf(stdout, "\t-C use console instead of TCP ports\n");
fprintf(stdout, "\t-I set instance of SITL (adds 10*instance to all port numbers)\n");
fprintf(stdout, "\t-s SPEEDUP simulation speedup\n");
fprintf(stdout, "\t-O ORIGIN set home location (lat,lng,alt,yaw)\n");
fprintf(stdout, "\t-M MODEL set simulation model\n");
fprintf(stdout, "\t-F FDMADDR set FDM UDP address (IPv4)\n");
}
static const struct {
@ -59,14 +59,14 @@ static const struct {
void SITL_State::_parse_command_line(int argc, char * const argv[])
{
int opt;
int opt;
const char *home_str = NULL;
const char *model_str = NULL;
float speedup = 1.0f;
signal(SIGFPE, _sig_fpe);
// No-op SIGPIPE handler
signal(SIGPIPE, SIG_IGN);
signal(SIGFPE, _sig_fpe);
// No-op SIGPIPE handler
signal(SIGPIPE, SIG_IGN);
setvbuf(stdout, (char *)0, _IONBF, 0);
setvbuf(stderr, (char *)0, _IONBF, 0);
@ -77,7 +77,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_simin_port = 5501;
_fdm_address = "127.0.0.1";
const struct GetOptLong::option options[] = {
const struct GetOptLong::option options[] = {
{"help", false, 0, 'h'},
{"wipe", false, 0, 'w'},
{"speedup", true, 0, 's'},
@ -90,40 +90,40 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{"home", true, 0, 'O'},
{"model", true, 0, 'M'},
{"frame", true, 0, 'F'},
{0, false, 0, 0}
};
{0, false, 0, 0}
};
GetOptLong gopt(argc, argv, "hws:r:H:CI:P:SO:M:F:",
options);
while ((opt = gopt.getoption()) != -1) {
switch (opt) {
case 'w':
AP_Param::erase_all();
unlink("dataflash.bin");
break;
case 'r':
_framerate = (unsigned)atoi(gopt.optarg);
break;
case 'H':
_initial_height = atof(gopt.optarg);
break;
case 'C':
HALSITL::SITLUARTDriver::_console = true;
break;
case 'I': {
switch (opt) {
case 'w':
AP_Param::erase_all();
unlink("dataflash.bin");
break;
case 'r':
_framerate = (unsigned)atoi(gopt.optarg);
break;
case 'H':
_initial_height = atof(gopt.optarg);
break;
case 'C':
HALSITL::SITLUARTDriver::_console = true;
break;
case 'I': {
uint8_t instance = atoi(gopt.optarg);
_base_port += instance * 10;
_rcout_port += instance * 10;
_simin_port += instance * 10;
}
break;
case 'P':
break;
case 'P':
_set_param_default(gopt.optarg);
break;
case 'S':
break;
case 'S':
_synthetic_clock_mode = true;
break;
break;
case 'O':
home_str = gopt.optarg;
break;
@ -136,11 +136,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
case 'F':
_fdm_address = gopt.optarg;
break;
default:
_usage();
exit(1);
}
}
default:
_usage();
exit(1);
}
}
if (model_str && home_str) {
for (uint8_t i=0; i<sizeof(model_constructors)/sizeof(model_constructors[0]); i++) {
@ -154,28 +154,28 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
}
}
fprintf(stdout, "Starting sketch '%s'\n", SKETCH);
fprintf(stdout, "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)
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)
pwm_input[2] = 1500;
} else {
_vehicle = ArduPlane;
if (_framerate == 0) {
_framerate = 50;
}
}
} else {
_vehicle = ArduPlane;
if (_framerate == 0) {
_framerate = 50;
}
}
_sitl_setup();
_sitl_setup();
}
#endif

22
libraries/AP_HAL_SITL/Scheduler.cpp
View File

@ -36,14 +36,14 @@ SITLScheduler::SITLScheduler(SITL_State *sitlState) :
void SITLScheduler::init(void *unused)
{
gettimeofday(&_sketch_start_time,NULL);
gettimeofday(&_sketch_start_time,NULL);
}
uint64_t SITLScheduler::_micros64()
{
struct timeval tp;
gettimeofday(&tp,NULL);
uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
struct timeval tp;
gettimeofday(&tp,NULL);
uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
return ret;
@ -67,9 +67,9 @@ uint64_t SITLScheduler::millis64()
if (stopped_clock_usec) {
return stopped_clock_usec/1000;
}
struct timeval tp;
gettimeofday(&tp,NULL);
uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
struct timeval tp;
gettimeofday(&tp,NULL);
uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
return ret;
@ -82,15 +82,15 @@ uint32_t SITLScheduler::millis()
void SITLScheduler::delay_microseconds(uint16_t usec)
{
uint64_t start = micros64();
uint64_t start = micros64();
uint64_t dtime;
while ((dtime=(micros64() - start) < usec)) {
while ((dtime=(micros64() - start) < usec)) {
if (stopped_clock_usec) {
_sitlState->wait_clock(start+usec);
} else {
usleep(usec - dtime);
}
}
}
}
void SITLScheduler::delay(uint16_t ms)
@ -107,7 +107,7 @@ void SITLScheduler::delay(uint16_t ms)
}
void SITLScheduler::register_delay_callback(AP_HAL::Proc proc,
uint16_t min_time_ms)
uint16_t min_time_ms)
{
_delay_cb = proc;
_min_delay_cb_ms = min_time_ms;

13
libraries/AP_HAL_SITL/Scheduler.h
View File

@ -39,14 +39,21 @@ public:
void reboot(bool hold_in_bootloader);
void panic(const prog_char_t *errormsg);
bool interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; }
bool interrupts_are_blocked(void) {
return _nested_atomic_ctr != 0;
}
void sitl_begin_atomic() { _nested_atomic_ctr++; }
void sitl_begin_atomic() {
_nested_atomic_ctr++;
}
void sitl_end_atomic();
// callable from interrupt handler
static uint64_t _micros64();
static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
static void timer_event() {
_run_timer_procs(true);
_run_io_procs(true);
}
private:
SITL_State *_sitlState;

20
libraries/AP_HAL_SITL/Storage.cpp
View File

@ -12,24 +12,24 @@ using namespace HALSITL;
void SITLEEPROMStorage::_eeprom_open(void)
{
if (_eeprom_fd == -1) {
_eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT, 0777);
assert(ftruncate(_eeprom_fd, HAL_STORAGE_SIZE) == 0);
}
if (_eeprom_fd == -1) {
_eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT, 0777);
assert(ftruncate(_eeprom_fd, HAL_STORAGE_SIZE) == 0);
}
}
void SITLEEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
{
assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE);
_eeprom_open();
assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE);
_eeprom_open();
assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
}
void SITLEEPROMStorage::write_block(uint16_t dst, const void *src, size_t n)
{
assert(dst < HAL_STORAGE_SIZE);
_eeprom_open();
assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n);
assert(dst < HAL_STORAGE_SIZE);
_eeprom_open();
assert(pwrite(_eeprom_fd, src, n, dst) == (ssize_t)n);
}
#endif

2
libraries/AP_HAL_SITL/Storage.h
View File

@ -9,7 +9,7 @@
class HALSITL::SITLEEPROMStorage : public AP_HAL::Storage {
public:
SITLEEPROMStorage() {
_eeprom_fd = -1;
_eeprom_fd = -1;
}
void init(void* machtnichts) {}
void read_block(void *dst, uint16_t src, size_t n);

172
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -188,79 +188,79 @@ size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size)
*/
void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
{
int one=1;
struct sockaddr_in sockaddr;
int ret;
int one=1;
struct sockaddr_in sockaddr;
int ret;
if (_connected) {
return;
}
if (_connected) {
return;
}
if (_console) {
// hack for console access
_connected = true;
_listen_fd = -1;
_fd = 1;
return;
}
if (_console) {
// hack for console access
_connected = true;
_listen_fd = -1;
_fd = 1;
return;
}
if (_fd != -1) {
close(_fd);
}
if (_fd != -1) {
close(_fd);
}
if (_listen_fd == -1) {
memset(&sockaddr,0,sizeof(sockaddr));
if (_listen_fd == -1) {
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(_sitlState->base_port() + _portNumber);
sockaddr.sin_family = AF_INET;
sockaddr.sin_port = htons(_sitlState->base_port() + _portNumber);
sockaddr.sin_family = AF_INET;
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_listen_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_listen_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
/* we want to be able to re-use ports quickly */
setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
/* we want to be able to re-use ports quickly */
setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
fprintf(stderr, "bind port %u for %u\n",
fprintf(stderr, "bind port %u for %u\n",
(unsigned)ntohs(sockaddr.sin_port),
(unsigned)_portNumber),
ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
(unsigned)_portNumber),
ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
ret = listen(_listen_fd, 5);
if (ret == -1) {
fprintf(stderr, "listen failed - %s\n", strerror(errno));
exit(1);
}
fprintf(stderr, "Serial port %u on TCP port %u\n", _portNumber,
_sitlState->base_port() + _portNumber);
fflush(stdout);
strerror(errno));
exit(1);
}
if (wait_for_connection) {
fprintf(stdout, "Waiting for connection ....\n");
fflush(stdout);
_fd = accept(_listen_fd, NULL, NULL);
if (_fd == -1) {
fprintf(stderr, "accept() error - %s", strerror(errno));
exit(1);
}
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
_connected = true;
ret = listen(_listen_fd, 5);
if (ret == -1) {
fprintf(stderr, "listen failed - %s\n", strerror(errno));
exit(1);
}
fprintf(stderr, "Serial port %u on TCP port %u\n", _portNumber,
_sitlState->base_port() + _portNumber);
fflush(stdout);
}
if (wait_for_connection) {
fprintf(stdout, "Waiting for connection ....\n");
fflush(stdout);
_fd = accept(_listen_fd, NULL, NULL);
if (_fd == -1) {
fprintf(stderr, "accept() error - %s", strerror(errno));
exit(1);
}
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
_connected = true;
}
}
/*
@ -268,20 +268,20 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
*/
void SITLUARTDriver::_check_connection(void)
{
if (_connected) {
// we only want 1 connection at a time
return;
}
if (_select_check(_listen_fd)) {
_fd = accept(_listen_fd, NULL, NULL);
if (_fd != -1) {
int one = 1;
_connected = true;
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
fprintf(stdout, "New connection on serial port %u\n", _portNumber);
}
}
if (_connected) {
// we only want 1 connection at a time
return;
}
if (_select_check(_listen_fd)) {
_fd = accept(_listen_fd, NULL, NULL);
if (_fd != -1) {
int one = 1;
_connected = true;
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
fprintf(stdout, "New connection on serial port %u\n", _portNumber);
}
}
}
/*
@ -289,26 +289,26 @@ void SITLUARTDriver::_check_connection(void)
*/
bool SITLUARTDriver::_select_check(int fd)
{
fd_set fds;
struct timeval tv;
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(fd, &fds);
FD_ZERO(&fds);
FD_SET(fd, &fds);
// zero time means immediate return from select()
tv.tv_sec = 0;
tv.tv_usec = 0;
// zero time means immediate return from select()
tv.tv_sec = 0;
tv.tv_usec = 0;
if (select(fd+1, &fds, NULL, NULL, &tv) == 1) {
return true;
}
return false;
if (select(fd+1, &fds, NULL, NULL, &tv) == 1) {
return true;
}
return false;
}
void SITLUARTDriver::_set_nonblocking(int fd)
{
unsigned v = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, v | O_NONBLOCK);
unsigned v = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, v | O_NONBLOCK);
}
#endif

48
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -13,30 +13,34 @@ class HALSITL::SITLUARTDriver : public AP_HAL::UARTDriver {
public:
friend class HALSITL::SITL_State;
SITLUARTDriver(const uint8_t portNumber, SITL_State *sitlState) {
_portNumber = portNumber;
SITLUARTDriver(const uint8_t portNumber, SITL_State *sitlState) {
_portNumber = portNumber;
_rxSpace = _default_rx_buffer_size;
_txSpace = _default_tx_buffer_size;
_sitlState = sitlState;
_fd = -1;
_listen_fd = -1;
}
}
/* Implementations of UARTDriver virtual methods */
void begin(uint32_t b) { begin(b, 0, 0); }
void begin(uint32_t b) {
begin(b, 0, 0);
}
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
void end();
void flush();
bool is_initialized() { return true; }
bool is_initialized() {
return true;
}
void set_blocking_writes(bool blocking)
{
_nonblocking_writes = !blocking;
_nonblocking_writes = !blocking;
}
bool tx_pending() {
return false;
return false;
}
/* Implementations of Stream virtual methods */
@ -49,15 +53,15 @@ public:
size_t write(const uint8_t *buffer, size_t size);
// file descriptor, exposed so SITL_State::loop_hook() can use it
int _fd;
int _fd;
private:
uint8_t _portNumber;
bool _connected; // true if a client has connected
int _listen_fd; // socket we are listening on
int _serial_port;
static bool _console;
bool _nonblocking_writes;
bool _connected; // true if a client has connected
int _listen_fd; // socket we are listening on
int _serial_port;
static bool _console;
bool _nonblocking_writes;
uint16_t _rxSpace;
uint16_t _txSpace;
@ -66,17 +70,17 @@ private:
static bool _select_check(int );
static void _set_nonblocking(int );
/// default receive buffer size
static const uint16_t _default_rx_buffer_size = 128;
/// default receive buffer size
static const uint16_t _default_rx_buffer_size = 128;
/// default transmit buffer size
static const uint16_t _default_tx_buffer_size = 16;
/// default transmit buffer size
static const uint16_t _default_tx_buffer_size = 16;
/// maxium tx/rx buffer size
/// @note if we could bring the max size down to 256, the mask and head/tail
/// pointers in the buffer could become uint8_t.
///
static const uint16_t _max_buffer_size = 512;
/// maxium tx/rx buffer size
/// @note if we could bring the max size down to 256, the mask and head/tail
/// pointers in the buffer could become uint8_t.
///
static const uint16_t _max_buffer_size = 512;
SITL_State *_sitlState;

4
libraries/AP_HAL_SITL/Util.h
View File

@ -7,7 +7,9 @@
class HALSITL::SITLUtil : public AP_HAL::Util {
public:
bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; }
bool run_debug_shell(AP_HAL::BetterStream *stream) {
return false;
}
};
#endif // __AP_HAL_SITL_UTIL_H__

14
libraries/AP_HAL_SITL/sitl_barometer.cpp
View File

@ -62,13 +62,13 @@ void SITL_State::_update_barometer(float altitude)
// storing data from sensor to buffer
if (now - last_store_time_baro >= 10) { // store data every 10 ms.
last_store_time_baro = now;
if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer
store_index_baro = 0;
}
buffer_baro[store_index_baro].data = sim_alt; // add data to current index
buffer_baro[store_index_baro].time = last_store_time_baro; // add time_stamp_baro to current index
store_index_baro = store_index_baro + 1; // increment index
last_store_time_baro = now;
if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer
store_index_baro = 0;
}
buffer_baro[store_index_baro].data = sim_alt; // add data to current index
buffer_baro[store_index_baro].time = last_store_time_baro; // add time_stamp_baro to current index
store_index_baro = store_index_baro + 1; // increment index
}
// return delayed measurement

674
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -36,9 +36,9 @@ static uint8_t gps_delay;
// state of GPS emulation
static struct gps_state {
/* pipe emulating UBLOX GPS serial stream */
int gps_fd, client_fd;
uint32_t last_update; // milliseconds
/* pipe emulating UBLOX GPS serial stream */
int gps_fd, client_fd;
uint32_t last_update; // milliseconds
} gps_state, gps2_state;
/*
@ -47,17 +47,17 @@ static struct gps_state {
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;
}
}
// 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);
return read(fd, buf, count);
}
/*
@ -65,17 +65,17 @@ ssize_t SITL_State::gps_read(int fd, void *buf, size_t count)
*/
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();
HALSITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps_state.client_fd;
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();
HALSITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps_state.client_fd;
}
/*
@ -83,17 +83,17 @@ int SITL_State::gps_pipe(void)
*/
int SITL_State::gps2_pipe(void)
{
int fd[2];
if (gps2_state.client_fd != 0) {
return gps2_state.client_fd;
}
pipe(fd);
gps2_state.gps_fd = fd[1];
gps2_state.client_fd = fd[0];
gps2_state.last_update = _scheduler->millis();
HALSITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps2_state.client_fd;
int fd[2];
if (gps2_state.client_fd != 0) {
return gps2_state.client_fd;
}
pipe(fd);
gps2_state.gps_fd = fd[1];
gps2_state.client_fd = fd[0];
gps2_state.last_update = _scheduler->millis();
HALSITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps2_state.client_fd;
}
/*
@ -101,21 +101,21 @@ int SITL_State::gps2_pipe(void)
*/
void SITL_State::_gps_write(const uint8_t *p, uint16_t size)
{
while (size--) {
if (_sitl->gps_byteloss > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < _sitl->gps_byteloss) {
// lose the byte
p++;
continue;
}
}
write(gps_state.gps_fd, p, 1);
while (size--) {
if (_sitl->gps_byteloss > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < _sitl->gps_byteloss) {
// lose the byte
p++;
continue;
}
}
write(gps_state.gps_fd, p, 1);
if (_sitl->gps2_enable) {
write(gps2_state.gps_fd, p, 1);
}
p++;
}
}
}
/*
@ -123,26 +123,26 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size)
*/
void SITL_State::_gps_send_ubx(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]);
}
_gps_write(hdr, sizeof(hdr));
_gps_write(buf, size);
_gps_write(chk, sizeof(chk));
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]);
}
_gps_write(hdr, sizeof(hdr));
_gps_write(buf, size);
_gps_write(chk, sizeof(chk));
}
/*
@ -163,111 +163,111 @@ static void gps_time(uint16_t *time_week, uint32_t *time_week_ms)
*/
void SITL_State::_update_gps_ubx(const struct gps_data *d)
{
struct PACKED 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 PACKED 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 PACKED 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 PACKED 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;
struct PACKED 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 PACKED 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 PACKED 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 PACKED 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;
uint16_t time_week;
uint32_t time_week_ms;
gps_time(&time_week, &time_week_ms);
pos.time = time_week_ms;
pos.longitude = d->longitude * 1.0e7;
pos.latitude = d->latitude * 1.0e7;
pos.altitude_ellipsoid = d->altitude*1000.0f;
pos.altitude_msl = d->altitude*1000.0f;
pos.horizontal_accuracy = 1500;
pos.vertical_accuracy = 2000;
pos.time = time_week_ms;
pos.longitude = d->longitude * 1.0e7;
pos.latitude = d->latitude * 1.0e7;
pos.altitude_ellipsoid = d->altitude*1000.0f;
pos.altitude_msl = d->altitude*1000.0f;
pos.horizontal_accuracy = 1500;
pos.vertical_accuracy = 2000;
status.time = time_week_ms;
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();
status.time = time_week_ms;
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 = time_week_ms;
velned.ned_north = 100.0f * d->speedN;
velned.ned_east = 100.0f * d->speedE;
velned.ned_down = 100.0f * d->speedD;
velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100;
velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100;
velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f;
if (velned.heading_2d < 0.0f) {
velned.heading_2d += 360.0f * 100000.0f;
}
velned.speed_accuracy = 40;
velned.heading_accuracy = 4;
velned.time = time_week_ms;
velned.ned_north = 100.0f * d->speedN;
velned.ned_east = 100.0f * d->speedE;
velned.ned_down = 100.0f * d->speedD;
velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100;
velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100;
velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f;
if (velned.heading_2d < 0.0f) {
velned.heading_2d += 360.0f * 100000.0f;
}
velned.speed_accuracy = 40;
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?_sitl->gps_numsats:3;
memset(&sol, 0, sizeof(sol));
sol.fix_type = d->have_lock?3:0;
sol.fix_status = 221;
sol.satellites = d->have_lock?_sitl->gps_numsats:3;
sol.time = time_week_ms;
sol.week = time_week;
_gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
_gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
_gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
_gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
_gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
_gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
}
static void swap_uint32(uint32_t *v, uint8_t n)
{
while (n--) {
*v = htonl(*v);
v++;
}
while (n--) {
*v = htonl(*v);
v++;
}
}
/*
@ -275,11 +275,11 @@ static void swap_uint32(uint32_t *v, uint8_t n)
*/
static void mtk_checksum(const uint8_t *data, uint8_t n, uint8_t *ck_a, uint8_t *ck_b)
{
*ck_a = *ck_b = 0;
while (n--) {
*ck_a += *data++;
*ck_b += *ck_a;
}
*ck_a = *ck_b = 0;
while (n--) {
*ck_a += *data++;
*ck_b += *ck_a;
}
}
@ -289,55 +289,55 @@ static void mtk_checksum(const uint8_t *data, uint8_t n, uint8_t *ck_a, uint8_t
void SITL_State::_update_gps_mtk(const struct gps_data *d)
{
struct PACKED mtk_msg {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
uint8_t ck_a;
uint8_t ck_b;
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
uint8_t ck_a;
uint8_t ck_b;
} p;
p.preamble1 = 0xb5;
p.preamble2 = 0x62;
p.msg_class = 1;
p.msg_id = 5;
p.preamble1 = 0xb5;
p.preamble2 = 0x62;
p.msg_class = 1;
p.msg_id = 5;
p.latitude = d->latitude * 1.0e6;
p.longitude = d->longitude * 1.0e6;
p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0f;
if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 1000000.0f;
}
if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 1000000.0f;
}
p.satellites = d->have_lock?_sitl->gps_numsats:3;
p.fix_type = d->have_lock?3:1;
// the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time,
// done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm;
struct timeval tv;
// the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time,
// done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm;
struct timeval tv;
gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec);
gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec);
uint32_t hsec = (tv.tv_usec / (10000*20)) * 20; // always multiple of 20
p.utc_time = hsec + tm.tm_sec*100 + tm.tm_min*100*100 + tm.tm_hour*100*100*100;
swap_uint32((uint32_t *)&p.latitude, 5);
swap_uint32((uint32_t *)&p.utc_time, 1);
mtk_checksum(&p.msg_class, sizeof(p)-4, &p.ck_a, &p.ck_b);
mtk_checksum(&p.msg_class, sizeof(p)-4, &p.ck_a, &p.ck_b);
_gps_write((uint8_t*)&p, sizeof(p));
_gps_write((uint8_t*)&p, sizeof(p));
}
/*
@ -346,9 +346,9 @@ void SITL_State::_update_gps_mtk(const struct gps_data *d)
void SITL_State::_update_gps_mtk16(const struct gps_data *d)
{
struct PACKED mtk_msg {
uint8_t preamble1;
uint8_t preamble2;
uint8_t size;
uint8_t preamble1;
uint8_t preamble2;
uint8_t size;
int32_t latitude;
int32_t longitude;
int32_t altitude;
@ -359,43 +359,43 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d)
uint32_t utc_date;
uint32_t utc_time;
uint16_t hdop;
uint8_t ck_a;
uint8_t ck_b;
uint8_t ck_a;
uint8_t ck_b;
} p;
p.preamble1 = 0xd0;
p.preamble2 = 0xdd;
p.size = sizeof(p) - 5;
p.preamble1 = 0xd0;
p.preamble2 = 0xdd;
p.size = sizeof(p) - 5;
p.latitude = d->latitude * 1.0e6;
p.longitude = d->longitude * 1.0e6;
p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f;
if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 100.0f;
}
if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 100.0f;
}
p.satellites = d->have_lock?_sitl->gps_numsats:3;
p.fix_type = d->have_lock?3:1;
// the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time,
// done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm;
struct timeval tv;
// the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time,
// done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm;
struct timeval tv;
gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec);
gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec);
uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200
p.utc_date = (tm.tm_year-100) + ((tm.tm_mon+1)*100) + (tm.tm_mday*100*100);
p.utc_time = millisec + tm.tm_sec*1000 + tm.tm_min*1000*100 + tm.tm_hour*1000*100*100;
p.hdop = 115;
p.hdop = 115;
mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);
mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);
_gps_write((uint8_t*)&p, sizeof(p));
_gps_write((uint8_t*)&p, sizeof(p));
}
/*
@ -404,9 +404,9 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d)
void SITL_State::_update_gps_mtk19(const struct gps_data *d)
{
struct PACKED mtk_msg {
uint8_t preamble1;
uint8_t preamble2;
uint8_t size;
uint8_t preamble1;
uint8_t preamble2;
uint8_t size;
int32_t latitude;
int32_t longitude;
int32_t altitude;
@ -417,43 +417,43 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d)
uint32_t utc_date;
uint32_t utc_time;
uint16_t hdop;
uint8_t ck_a;
uint8_t ck_b;
uint8_t ck_a;
uint8_t ck_b;
} p;
p.preamble1 = 0xd1;
p.preamble2 = 0xdd;
p.size = sizeof(p) - 5;
p.preamble1 = 0xd1;
p.preamble2 = 0xdd;
p.size = sizeof(p) - 5;
p.latitude = d->latitude * 1.0e7;
p.longitude = d->longitude * 1.0e7;
p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f;
if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 100.0f;
}
if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 100.0f;
}
p.satellites = d->have_lock?_sitl->gps_numsats:3;
p.fix_type = d->have_lock?3:1;
// the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time,
// done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm;
struct timeval tv;
// the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time,
// done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm;
struct timeval tv;
gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec);
gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec);
uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200
p.utc_date = (tm.tm_year-100) + ((tm.tm_mon+1)*100) + (tm.tm_mday*100*100);
p.utc_time = millisec + tm.tm_sec*1000 + tm.tm_min*1000*100 + tm.tm_hour*1000*100*100;
p.hdop = 115;
p.hdop = 115;
mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);
mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);
_gps_write((uint8_t*)&p, sizeof(p));
_gps_write((uint8_t*)&p, sizeof(p));
}
/*
@ -552,25 +552,25 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d)
void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
{
if (len != 0 && payload == 0) {
return; //SBP_NULL_ERROR;
}
if (len != 0 && payload == 0) {
return; //SBP_NULL_ERROR;
}
uint8_t preamble = 0x55;
_gps_write(&preamble, 1);
_gps_write((uint8_t*)&msg_type, 2);
_gps_write((uint8_t*)&sender_id, 2);
_gps_write(&len, 1);
if (len > 0) {
_gps_write((uint8_t*)payload, len);
}
uint8_t preamble = 0x55;
_gps_write(&preamble, 1);
_gps_write((uint8_t*)&msg_type, 2);
_gps_write((uint8_t*)&sender_id, 2);
_gps_write(&len, 1);
if (len > 0) {
_gps_write((uint8_t*)payload, len);
}
uint16_t crc;
crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
_gps_write((uint8_t*)&crc, 2);
uint16_t crc;
crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
_gps_write((uint8_t*)&crc, 2);
}
@ -629,7 +629,7 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d)
_sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) {
return;
return;
}
pos.tow = time_week_ms;
@ -660,19 +660,19 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d)
static uint32_t do_every_count = 0;
if (do_every_count % 5 == 0) {
dops.tow = time_week_ms;
dops.gdop = 1;
dops.pdop = 1;
dops.tdop = 1;
dops.hdop = 100;
dops.vdop = 1;
_sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops);
dops.tow = time_week_ms;
dops.gdop = 1;
dops.pdop = 1;
dops.tdop = 1;
dops.hdop = 100;
dops.vdop = 1;
_sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops);
uint32_t system_flags = 0;
_sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222,
sizeof(system_flags),
(uint8_t*)&system_flags);
uint32_t system_flags = 0;
_sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222,
sizeof(system_flags),
(uint8_t*)&system_flags);
}
do_every_count++;
@ -682,106 +682,106 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d)
possibly send a new GPS packet
*/
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock)
double speedN, double speedE, double speedD, bool have_lock)
{
struct gps_data d;
char c;
struct gps_data d;
char c;
Vector3f glitch_offsets = _sitl->gps_glitch;
//Capture current position as basestation location for
if (!_gps_has_basestation_position) {
if (have_lock) {
_gps_basestation_data.latitude = latitude;
_gps_basestation_data.longitude = longitude;
_gps_basestation_data.altitude = altitude;
_gps_basestation_data.speedN = speedN;
_gps_basestation_data.speedE = speedE;
_gps_basestation_data.speedD = speedD;
_gps_basestation_data.have_lock = have_lock;
_gps_has_basestation_position = true;
}
if (have_lock) {
_gps_basestation_data.latitude = latitude;
_gps_basestation_data.longitude = longitude;
_gps_basestation_data.altitude = altitude;
_gps_basestation_data.speedN = speedN;
_gps_basestation_data.speedE = speedE;
_gps_basestation_data.speedD = speedD;
_gps_basestation_data.have_lock = have_lock;
_gps_has_basestation_position = true;
}
}
// run at configured GPS rate (default 5Hz)
if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
return;
}
// run at configured GPS rate (default 5Hz)
if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
return;
}
// swallow any config bytes
if (gps_state.gps_fd != 0) {
read(gps_state.gps_fd, &c, 1);
}
if (gps2_state.gps_fd != 0) {
read(gps2_state.gps_fd, &c, 1);
}
// swallow any config bytes
if (gps_state.gps_fd != 0) {
read(gps_state.gps_fd, &c, 1);
}
if (gps2_state.gps_fd != 0) {
read(gps2_state.gps_fd, &c, 1);
}
gps_state.last_update = hal.scheduler->millis();
gps2_state.last_update = hal.scheduler->millis();
gps_state.last_update = hal.scheduler->millis();
gps2_state.last_update = hal.scheduler->millis();
d.latitude = latitude + glitch_offsets.x;
d.longitude = longitude + glitch_offsets.y;
d.altitude = altitude + glitch_offsets.z;
d.latitude = latitude + glitch_offsets.x;
d.longitude = longitude + glitch_offsets.y;
d.altitude = altitude + glitch_offsets.z;
if (_sitl->gps_drift_alt > 0) {
// slow altitude drift
d.altitude += _sitl->gps_drift_alt*sinf(hal.scheduler->millis()*0.001f*0.02f);
}
d.speedN = speedN;
d.speedE = speedE;
d.speedD = speedD;
d.have_lock = have_lock;
d.speedN = speedN;
d.speedE = speedE;
d.speedD = speedD;
d.have_lock = have_lock;
// add in some GPS lag
_gps_data[next_gps_index++] = d;
if (next_gps_index >= gps_delay+1) {
next_gps_index = 0;
}
// add in some GPS lag
_gps_data[next_gps_index++] = d;
if (next_gps_index >= gps_delay+1) {
next_gps_index = 0;
}
d = _gps_data[next_gps_index];
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;
}
}
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;
}
}
if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
return;
}
if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
return;
}
switch ((SITL::GPSType)_sitl->gps_type.get()) {
case SITL::GPS_TYPE_NONE:
// no GPS attached
break;
switch ((SITL::GPSType)_sitl->gps_type.get()) {
case SITL::GPS_TYPE_NONE:
// no GPS attached
break;
case SITL::GPS_TYPE_UBLOX:
_update_gps_ubx(&d);
break;
case SITL::GPS_TYPE_UBLOX:
_update_gps_ubx(&d);
break;
case SITL::GPS_TYPE_MTK:
_update_gps_mtk(&d);
break;
case SITL::GPS_TYPE_MTK:
_update_gps_mtk(&d);
break;
case SITL::GPS_TYPE_MTK16:
_update_gps_mtk16(&d);
break;
case SITL::GPS_TYPE_MTK16:
_update_gps_mtk16(&d);
break;
case SITL::GPS_TYPE_MTK19:
_update_gps_mtk19(&d);
break;
case SITL::GPS_TYPE_MTK19:
_update_gps_mtk19(&d);
break;
case SITL::GPS_TYPE_NMEA:
_update_gps_nmea(&d);
break;
case SITL::GPS_TYPE_NMEA:
_update_gps_nmea(&d);
break;
case SITL::GPS_TYPE_SBP:
_update_gps_sbp(&d);
break;
case SITL::GPS_TYPE_SBP:
_update_gps_sbp(&d);
break;
}
}
}
#endif

136
libraries/AP_HAL_SITL/sitl_ins.cpp
View File

@ -33,15 +33,15 @@ using namespace HALSITL;
*/
uint16_t SITL_State::_airspeed_sensor(float airspeed)
{
const float airspeed_ratio = 1.9936f;
const float airspeed_offset = 2013;
float airspeed_pressure, airspeed_raw;
const float airspeed_ratio = 1.9936f;
const float airspeed_offset = 2013;
float airspeed_pressure, airspeed_raw;
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
airspeed_raw = airspeed_pressure + airspeed_offset;
if (airspeed_raw/4 > 0xFFFF) {
return 0xFFFF;
}
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
airspeed_raw = airspeed_pressure + airspeed_offset;
if (airspeed_raw/4 > 0xFFFF) {
return 0xFFFF;
}
// add delay
uint32_t now = hal.scheduler->millis();
uint32_t best_time_delta_wind = 200; // initialise large time representing buffer entry closest to current time - delay.
@ -73,22 +73,22 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed)
airspeed_raw = buffer_wind[best_index_wind].data;
}
return airspeed_raw/4;
return airspeed_raw/4;
}
float SITL_State::_gyro_drift(void)
{
if (_sitl->drift_speed == 0.0f ||
_sitl->drift_time == 0.0f) {
return 0;
}
double period = _sitl->drift_time * 2;
double minutes = fmod(_scheduler->_micros64() / 60.0e6, period);
if (minutes < period/2) {
return minutes * ToRad(_sitl->drift_speed);
}
return (period - minutes) * ToRad(_sitl->drift_speed);
_sitl->drift_time == 0.0f) {
return 0;
}
double period = _sitl->drift_time * 2;
double minutes = fmod(_scheduler->_micros64() / 60.0e6, period);
if (minutes < period/2) {
return minutes * ToRad(_sitl->drift_speed);
}
return (period - minutes) * ToRad(_sitl->drift_speed);
}
@ -101,7 +101,7 @@ uint16_t SITL_State::_ground_sonar(void)
float voltage = 5.0f;
if (fabsf(_sitl->state.rollDeg) < 90 &&
fabsf(_sitl->state.pitchDeg) < 90) {
fabsf(_sitl->state.pitchDeg) < 90) {
// adjust for apparent altitude with roll
altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
@ -147,68 +147,68 @@ uint16_t SITL_State::_ground_sonar(void)
*/
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, float altitude)
double rollRate, double pitchRate,double yawRate, // Local to plane
double xAccel, double yAccel, double zAccel, // Local to plane
float airspeed, float altitude)
{
double p, q, r;
double p, q, r;
if (_ins == NULL) {
// no inertial sensor in this sketch
return;
}
if (_ins == NULL) {
// no inertial sensor in this sketch
return;
}
SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);
SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);
// minimum noise levels are 2 bits, but averaged over many
// samples, giving around 0.01 m/s/s
float accel_noise = 0.01f;
// minimum gyro noise is also less than 1 bit
float gyro_noise = ToRad(0.04f);
if (_motors_on) {
// add extra noise when the motors are on
accel_noise += _sitl->accel_noise;
gyro_noise += ToRad(_sitl->gyro_noise);
}
// get accel bias (add only to first accelerometer)
Vector3f accel_bias = _sitl->accel_bias.get();
float xAccel1 = xAccel + accel_noise * _rand_float() + accel_bias.x;
float yAccel1 = yAccel + accel_noise * _rand_float() + accel_bias.y;
float zAccel1 = zAccel + accel_noise * _rand_float() + accel_bias.z;
// minimum noise levels are 2 bits, but averaged over many
// samples, giving around 0.01 m/s/s
float accel_noise = 0.01f;
// minimum gyro noise is also less than 1 bit
float gyro_noise = ToRad(0.04f);
if (_motors_on) {
// add extra noise when the motors are on
accel_noise += _sitl->accel_noise;
gyro_noise += ToRad(_sitl->gyro_noise);
}
// get accel bias (add only to first accelerometer)
Vector3f accel_bias = _sitl->accel_bias.get();
float xAccel1 = xAccel + accel_noise * _rand_float() + accel_bias.x;
float yAccel1 = yAccel + accel_noise * _rand_float() + accel_bias.y;
float zAccel1 = zAccel + accel_noise * _rand_float() + accel_bias.z;
float xAccel2 = xAccel + accel_noise * _rand_float();
float yAccel2 = yAccel + accel_noise * _rand_float();
float zAccel2 = zAccel + accel_noise * _rand_float();
float xAccel2 = xAccel + accel_noise * _rand_float();
float yAccel2 = yAccel + accel_noise * _rand_float();
float zAccel2 = zAccel + accel_noise * _rand_float();
if (fabs(_sitl->accel_fail) > 1.0e-6) {
xAccel1 = _sitl->accel_fail;
yAccel1 = _sitl->accel_fail;
zAccel1 = _sitl->accel_fail;
}
if (fabs(_sitl->accel_fail) > 1.0e-6) {
xAccel1 = _sitl->accel_fail;
yAccel1 = _sitl->accel_fail;
zAccel1 = _sitl->accel_fail;
}
_ins->set_accel(0, Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0));
_ins->set_accel(1, Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1));
_ins->set_accel(0, Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0));
_ins->set_accel(1, Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1));
p += _gyro_drift();
q += _gyro_drift();
r += _gyro_drift();
p += _gyro_drift();
q += _gyro_drift();
r += _gyro_drift();
float p1 = p + gyro_noise * _rand_float();
float q1 = q + gyro_noise * _rand_float();
float r1 = r + gyro_noise * _rand_float();
float p1 = p + gyro_noise * _rand_float();
float q1 = q + gyro_noise * _rand_float();
float r1 = r + gyro_noise * _rand_float();
float p2 = p + gyro_noise * _rand_float();
float q2 = q + gyro_noise * _rand_float();
float r2 = r + gyro_noise * _rand_float();
float p2 = p + gyro_noise * _rand_float();
float q2 = q + gyro_noise * _rand_float();
float r2 = r + gyro_noise * _rand_float();
_ins->set_gyro(0, Vector3f(p1, q1, r1) + _ins->get_gyro_offsets(0));
_ins->set_gyro(1, Vector3f(p2, q2, r2) + _ins->get_gyro_offsets(1));
_ins->set_gyro(0, Vector3f(p1, q1, r1) + _ins->get_gyro_offsets(0));
_ins->set_gyro(1, Vector3f(p2, q2, r2) + _ins->get_gyro_offsets(1));
sonar_pin_value = _ground_sonar();
airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
sonar_pin_value = _ground_sonar();
airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
}
#endif

2
libraries/AP_HAL_SITL/sitl_optical_flow.cpp
View File

@ -38,7 +38,7 @@ void SITL_State::_update_flow(void)
static uint32_t last_flow_ms;
if (!_optical_flow ||
!_sitl->flow_enable) {
!_sitl->flow_enable) {
return;
}