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__ #define __AP_HAL_SITL_NAMESPACE_H__
namespace HALSITL { namespace HALSITL {
class SITLUARTDriver; class SITLUARTDriver;
class SITLScheduler; class SITLScheduler;
class SITL_State; class SITL_State;
class SITLEEPROMStorage; class SITLEEPROMStorage;
class SITLAnalogIn; class SITLAnalogIn;
class SITLRCInput; class SITLRCInput;
class SITLRCOutput; class SITLRCOutput;
class ADCSource; class ADCSource;
class RCInput; class RCInput;
class SITLUtil; class SITLUtil;
} }
#endif // __AP_HAL_SITL_NAMESPACE_H__ #endif // __AP_HAL_SITL_NAMESPACE_H__

10
libraries/AP_HAL_SITL/AnalogIn.cpp
View File

@ -17,22 +17,22 @@ ADCSource::ADCSource(SITL_State *sitlState, uint8_t pin) :
{} {}
float ADCSource::read_average() { float ADCSource::read_average() {
return read_latest(); return read_latest();
} }
float ADCSource::voltage_average() { float ADCSource::voltage_average() {
return (5.0f/1023.0f) * read_average(); return (5.0f/1023.0f) * read_average();
} }
float ADCSource::voltage_latest() { float ADCSource::voltage_latest() {
return (5.0f/1023.0f) * read_latest(); return (5.0f/1023.0f) * read_latest();
} }
float ADCSource::read_latest() { float ADCSource::read_latest() {
switch (_pin) { switch (_pin) {
case ANALOG_INPUT_BOARD_VCC: case ANALOG_INPUT_BOARD_VCC:
return 1023; return 1023;
case 0: case 0:
return _sitlState->sonar_pin_value; return _sitlState->sonar_pin_value;
@ -59,7 +59,7 @@ void SITLAnalogIn::init(void *ap_hal_scheduler) {
} }
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) { AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) {
return new ADCSource(_sitlState, pin); return new ADCSource(_sitlState, pin);
} }
#endif #endif

12
libraries/AP_HAL_SITL/AnalogIn.h
View File

@ -19,7 +19,9 @@ public:
void set_pin(uint8_t p); void set_pin(uint8_t p);
float voltage_average(); float voltage_average();
float voltage_latest(); 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_stop_pin(uint8_t pin) {}
void set_settle_time(uint16_t settle_time_ms) {} void set_settle_time(uint16_t settle_time_ms) {}
@ -28,16 +30,18 @@ private:
uint8_t _pin; uint8_t _pin;
}; };
/* SITLAnalogIn : a concrete class providing the implementations of the /* SITLAnalogIn : a concrete class providing the implementations of the
* timer event and the AP_HAL::AnalogIn interface */ * timer event and the AP_HAL::AnalogIn interface */
class HALSITL::SITLAnalogIn : public AP_HAL::AnalogIn { class HALSITL::SITLAnalogIn : public AP_HAL::AnalogIn {
public: public:
SITLAnalogIn(SITL_State *sitlState) { SITLAnalogIn(SITL_State *sitlState) {
_sitlState = sitlState; _sitlState = sitlState;
} }
void init(void* ap_hal_scheduler); void init(void* ap_hal_scheduler);
AP_HAL::AnalogSource* channel(int16_t n); AP_HAL::AnalogSource* channel(int16_t n);
float board_voltage(void) { return 5.0f; } float board_voltage(void) {
return 5.0f;
}
private: private:
static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS]; static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS];
SITL_State *_sitlState; SITL_State *_sitlState;

4
libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
View File

@ -44,7 +44,7 @@ static SITLUtil utilInstance;
HAL_SITL::HAL_SITL() : HAL_SITL::HAL_SITL() :
AP_HAL::HAL( AP_HAL::HAL(
&sitlUart0Driver, /* uartA */ &sitlUart0Driver, /* uartA */
&sitlUart1Driver, /* uartB */ &sitlUart1Driver, /* uartB */
&sitlUart2Driver, /* uartC */ &sitlUart2Driver, /* uartC */
&sitlUart3Driver, /* uartD */ &sitlUart3Driver, /* uartD */
@ -62,7 +62,7 @@ HAL_SITL::HAL_SITL() :
_sitl_state(&sitlState) _sitl_state(&sitlState)
{} {}
void HAL_SITL::init(int argc, char * const argv[]) const void HAL_SITL::init(int argc, char * const argv[]) const
{ {
_sitl_state->init(argc, argv); _sitl_state->init(argc, argv);
scheduler->init(NULL); scheduler->init(NULL);

2
libraries/AP_HAL_SITL/HAL_SITL_Class.h
View File

@ -12,7 +12,7 @@
class HAL_SITL : public AP_HAL::HAL { class HAL_SITL : public AP_HAL::HAL {
public: public:
HAL_SITL(); HAL_SITL();
void init(int argc, char * const argv[]) const; void init(int argc, char * const argv[]) const;
private: private:

6
libraries/AP_HAL_SITL/RCInput.cpp
View File

@ -12,7 +12,7 @@ void SITLRCInput::init(void* machtnichts)
clear_overrides(); clear_overrides();
} }
bool SITLRCInput::new_input() bool SITLRCInput::new_input()
{ {
if (_sitlState->new_rc_input) { if (_sitlState->new_rc_input) {
_sitlState->new_rc_input = false; _sitlState->new_rc_input = false;
@ -30,7 +30,7 @@ uint16_t SITLRCInput::read(uint8_t ch) {
uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) { uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) {
for (uint8_t i=0; i<len; i++) { 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; return 8;
} }
@ -57,7 +57,7 @@ bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
void SITLRCInput::clear_overrides() void SITLRCInput::clear_overrides()
{ {
for (uint8_t i = 0; i < 8; i++) { for (uint8_t i = 0; i < 8; i++) {
_override[i] = 0; _override[i] = 0;
} }
} }
#endif #endif

6
libraries/AP_HAL_SITL/RCInput.h
View File

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

10
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) void SITLRCOutput::write(uint8_t ch, uint16_t period_us)
{ {
if (ch < 11) { 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) 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) uint16_t SITLRCOutput::read(uint8_t ch)
{ {
if (ch < 11) { if (ch < 11) {
return _sitlState->pwm_output[ch]; return _sitlState->pwm_output[ch];
} }
return 0; return 0;
} }
void SITLRCOutput::read(uint16_t* period_us, uint8_t len) 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 #endif

6
libraries/AP_HAL_SITL/RCOutput.h
View File

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

314
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -37,7 +37,7 @@ void SITL_State::_set_param_default(const char *parm)
AP_Param *vp = AP_Param::find(parm, &var_type); AP_Param *vp = AP_Param::find(parm, &var_type);
if (vp == NULL) { if (vp == NULL) {
printf("Unknown parameter %s\n", parm); printf("Unknown parameter %s\n", parm);
exit(1); exit(1);
} }
if (var_type == AP_PARAM_FLOAT) { if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(value); ((AP_Float *)vp)->set_and_save(value);
@ -62,24 +62,24 @@ void SITL_State::_set_param_default(const char *parm)
void SITL_State::_sitl_setup(void) void SITL_State::_sitl_setup(void)
{ {
#ifndef __CYGWIN__ #ifndef __CYGWIN__
_parent_pid = getppid(); _parent_pid = getppid();
#endif #endif
_rcout_addr.sin_family = AF_INET; _rcout_addr.sin_family = AF_INET;
_rcout_addr.sin_port = htons(_rcout_port); _rcout_addr.sin_port = htons(_rcout_port);
inet_pton(AF_INET, _fdm_address, &_rcout_addr.sin_addr); inet_pton(AF_INET, _fdm_address, &_rcout_addr.sin_addr);
#ifndef HIL_MODE #ifndef HIL_MODE
_setup_fdm(); _setup_fdm();
#endif #endif
fprintf(stdout, "Starting SITL input\n"); fprintf(stdout, "Starting SITL input\n");
// find the barometer object if it exists // find the barometer object if it exists
_sitl = (SITL *)AP_Param::find_object("SIM_"); _sitl = (SITL *)AP_Param::find_object("SIM_");
_barometer = (AP_Baro *)AP_Param::find_object("GND_"); _barometer = (AP_Baro *)AP_Param::find_object("GND_");
_ins = (AP_InertialSensor *)AP_Param::find_object("INS_"); _ins = (AP_InertialSensor *)AP_Param::find_object("INS_");
_compass = (Compass *)AP_Param::find_object("COMPASS_"); _compass = (Compass *)AP_Param::find_object("COMPASS_");
_terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_"); _terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
_optical_flow = (OpticalFlow *)AP_Param::find_object("FLOW"); _optical_flow = (OpticalFlow *)AP_Param::find_object("FLOW");
if (_sitl != NULL) { if (_sitl != NULL) {
// setup some initial values // setup some initial values
@ -104,34 +104,34 @@ void SITL_State::_sitl_setup(void)
*/ */
void SITL_State::_setup_fdm(void) void SITL_State::_setup_fdm(void)
{ {
int one=1, ret; int one=1, ret;
struct sockaddr_in sockaddr; struct sockaddr_in sockaddr;
memset(&sockaddr,0,sizeof(sockaddr)); memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN #ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr); sockaddr.sin_len = sizeof(sockaddr);
#endif #endif
sockaddr.sin_port = htons(_simin_port); sockaddr.sin_port = htons(_simin_port);
sockaddr.sin_family = AF_INET; sockaddr.sin_family = AF_INET;
_sitl_fd = socket(AF_INET, SOCK_DGRAM, 0); _sitl_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (_sitl_fd == -1) { if (_sitl_fd == -1) {
fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno)); fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno));
exit(1); exit(1);
} }
/* we want to be able to re-use ports quickly */ /* we want to be able to re-use ports quickly */
setsockopt(_sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); setsockopt(_sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
ret = bind(_sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); ret = bind(_sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) { if (ret == -1) {
fprintf(stderr, "SITL: bind failed on port %u - %s\n", fprintf(stderr, "SITL: bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port), strerror(errno)); (unsigned)ntohs(sockaddr.sin_port), strerror(errno));
exit(1); exit(1);
} }
HALSITL::SITLUARTDriver::_set_nonblocking(_sitl_fd); HALSITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
} }
#endif #endif
@ -150,7 +150,7 @@ void SITL_State::_fdm_input_step(void)
} else { } else {
tv.tv_sec = 1; tv.tv_sec = 1;
tv.tv_usec = 0; tv.tv_usec = 0;
FD_ZERO(&fds); FD_ZERO(&fds);
FD_SET(_sitl_fd, &fds); FD_SET(_sitl_fd, &fds);
if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) { if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
@ -158,7 +158,7 @@ void SITL_State::_fdm_input_step(void)
_simulator_output(true); _simulator_output(true);
return; return;
} }
/* check for packet from flight sim */ /* check for packet from flight sim */
_fdm_input(); _fdm_input();
} }
@ -167,7 +167,7 @@ void SITL_State::_fdm_input_step(void)
if (kill(_parent_pid, 0) != 0) { if (kill(_parent_pid, 0) != 0) {
exit(1); exit(1);
} }
if (_scheduler->interrupts_are_blocked() || _sitl == NULL) { if (_scheduler->interrupts_are_blocked() || _sitl == NULL) {
return; return;
} }
@ -201,8 +201,8 @@ void SITL_State::_fdm_input_step(void)
_update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg); _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
_update_flow(); _update_flow();
} }
// trigger all APM timers. // trigger all APM timers.
_scheduler->timer_event(); _scheduler->timer_event();
_scheduler->sitl_end_atomic(); _scheduler->sitl_end_atomic();
} }
@ -221,37 +221,37 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
*/ */
void SITL_State::_fdm_input(void) void SITL_State::_fdm_input(void)
{ {
ssize_t size; ssize_t size;
struct pwm_packet { struct pwm_packet {
uint16_t pwm[8]; uint16_t pwm[8];
}; };
union { union {
struct sitl_fdm fg_pkt; struct sitl_fdm fg_pkt;
struct pwm_packet pwm_pkt; struct pwm_packet pwm_pkt;
} d; } d;
bool got_fg_input = false; bool got_fg_input = false;
size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT); size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
switch (size) { switch (size) {
case 148: case 148:
static uint32_t last_report; static uint32_t last_report;
static uint32_t count; static uint32_t count;
if (d.fg_pkt.magic != 0x4c56414f) { if (d.fg_pkt.magic != 0x4c56414f) {
fprintf(stdout, "Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic); fprintf(stdout, "Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic);
return; return;
} }
hal.scheduler->stop_clock(d.fg_pkt.timestamp_us); hal.scheduler->stop_clock(d.fg_pkt.timestamp_us);
_synthetic_clock_mode = true; _synthetic_clock_mode = true;
got_fg_input = true; got_fg_input = true;
if (d.fg_pkt.latitude == 0 || if (d.fg_pkt.latitude == 0 ||
d.fg_pkt.longitude == 0 || d.fg_pkt.longitude == 0 ||
d.fg_pkt.altitude <= 0) { d.fg_pkt.altitude <= 0) {
// garbage input // garbage input
return; return;
} }
if (_sitl != NULL) { if (_sitl != NULL) {
_sitl->state = d.fg_pkt; _sitl->state = d.fg_pkt;
@ -263,28 +263,28 @@ void SITL_State::_fdm_input(void)
} }
} }
} }
_update_count++; _update_count++;
count++; count++;
if (hal.scheduler->millis() - last_report > 1000) { if (hal.scheduler->millis() - last_report > 1000) {
//fprintf(stdout, "SIM %u FPS\n", count); //fprintf(stdout, "SIM %u FPS\n", count);
count = 0; count = 0;
last_report = hal.scheduler->millis(); last_report = hal.scheduler->millis();
} }
break; break;
case 16: { case 16: {
// a packet giving the receiver PWM inputs // a packet giving the receiver PWM inputs
uint8_t i; uint8_t i;
for (i=0; i<8; i++) { for (i=0; i<8; i++) {
// setup the pwn input for the RC channel inputs // setup the pwn input for the RC channel inputs
if (d.pwm_pkt.pwm[i] != 0) { if (d.pwm_pkt.pwm[i] != 0) {
pwm_input[i] = d.pwm_pkt.pwm[i]; pwm_input[i] = d.pwm_pkt.pwm[i];
} }
} }
break; break;
} }
} }
if (got_fg_input) { if (got_fg_input) {
// send RC output to flight sim // 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; static uint32_t last_update_usec;
/* this maps the registers used for PWM outputs. The RC /* this maps the registers used for PWM outputs. The RC
* driver updates these whenever it wants the channel output * driver updates these whenever it wants the channel output
* to change */ * to change */
uint8_t i; uint8_t i;
if (last_update_usec == 0) { if (last_update_usec == 0) {
for (i=0; i<11; i++) { for (i=0; i<11; i++) {
pwm_output[i] = 1000; pwm_output[i] = 1000;
} }
if (_vehicle == ArduPlane) { if (_vehicle == ArduPlane) {
pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500; pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500;
pwm_output[7] = 1800; pwm_output[7] = 1800;
} }
if (_vehicle == APMrover2) { if (_vehicle == APMrover2) {
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500; pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500;
pwm_output[7] = 1800; pwm_output[7] = 1800;
} }
for (i=0; i<11; i++) { for (i=0; i<11; i++) {
last_pwm_output[i] = pwm_output[i]; last_pwm_output[i] = pwm_output[i];
} }
} }
// output at chosen framerate // output at chosen framerate
uint32_t now = hal.scheduler->micros(); uint32_t now = hal.scheduler->micros();
float deltat = (now - last_update_usec) * 1.0e-6f; float deltat = (now - last_update_usec) * 1.0e-6f;
last_update_usec = now; last_update_usec = now;
_apply_servo_filter(deltat); _apply_servo_filter(deltat);
for (i=0; i<11; i++) { for (i=0; i<11; i++) {
if (pwm_output[i] == 0xFFFF) { if (pwm_output[i] == 0xFFFF) {
input.servos[i] = 0; input.servos[i] = 0;
} else { } else {
input.servos[i] = pwm_output[i]; input.servos[i] = pwm_output[i];
} }
last_pwm_output[i] = pwm_output[i]; last_pwm_output[i] = pwm_output[i];
} }
if (_vehicle == ArduPlane) { if (_vehicle == ArduPlane) {
// add in engine multiplier // add in engine multiplier
if (input.servos[2] > 1000) { if (input.servos[2] > 1000) {
input.servos[2] = ((input.servos[2]-1000) * _sitl->engine_mul) + 1000; input.servos[2] = ((input.servos[2]-1000) * _sitl->engine_mul) + 1000;
if (input.servos[2] > 2000) input.servos[2] = 2000; if (input.servos[2] > 2000) input.servos[2] = 2000;
} }
_motors_on = ((input.servos[2]-1000)/1000.0f) > 0; _motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
} else if (_vehicle == APMrover2) { } else if (_vehicle == APMrover2) {
// add in engine multiplier // add in engine multiplier
if (input.servos[2] != 1500) { if (input.servos[2] != 1500) {
input.servos[2] = ((input.servos[2]-1500) * _sitl->engine_mul) + 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] > 2000) input.servos[2] = 2000;
if (input.servos[2] < 1000) input.servos[2] = 1000; if (input.servos[2] < 1000) input.servos[2] = 1000;
} }
_motors_on = ((input.servos[2]-1500)/500.0f) != 0; _motors_on = ((input.servos[2]-1500)/500.0f) != 0;
} else { } else {
_motors_on = false; _motors_on = false;
// apply engine multiplier to first motor // apply engine multiplier to first motor
input.servos[0] = ((input.servos[0]-1000) * _sitl->engine_mul) + 1000; input.servos[0] = ((input.servos[0]-1000) * _sitl->engine_mul) + 1000;
// run checks on each motor // run checks on each motor
for (i=0; i<4; i++) { for (i=0; i<4; i++) {
// check motors do not exceed their limits // check motors do not exceed their limits
if (input.servos[i] > 2000) input.servos[i] = 2000; if (input.servos[i] > 2000) input.servos[i] = 2000;
if (input.servos[i] < 1000) input.servos[i] = 1000; if (input.servos[i] < 1000) input.servos[i] = 1000;
// update motor_on flag // update motor_on flag
if ((input.servos[i]-1000)/1000.0f > 0) { if ((input.servos[i]-1000)/1000.0f > 0) {
_motors_on = true; _motors_on = true;
} }
} }
} }
float throttle = _motors_on?(input.servos[2]-1000) / 1000.0f:0; float throttle = _motors_on?(input.servos[2]-1000) / 1000.0f:0;
// lose 0.7V at full throttle // 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) void SITL_State::_simulator_output(bool synthetic_clock_mode)
{ {
struct { struct {
uint16_t pwm[11]; uint16_t pwm[11];
uint16_t speed, direction, turbulance; uint16_t speed, direction, turbulance;
} control; } control;
Aircraft::sitl_input input; Aircraft::sitl_input input;
_simulator_servos(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)); memcpy(control.pwm, input.servos, sizeof(control.pwm));
// setup wind control // setup wind control
float wind_speed = _sitl->wind_speed * 100; float wind_speed = _sitl->wind_speed * 100;
float altitude = _barometer?_barometer->get_altitude():0; float altitude = _barometer?_barometer->get_altitude():0;
if (altitude < 0) { if (altitude < 0) {
@ -463,20 +463,20 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode)
if (altitude < 60) { if (altitude < 60) {
wind_speed *= altitude / 60.0f; wind_speed *= altitude / 60.0f;
} }
control.speed = wind_speed; control.speed = wind_speed;
float direction = _sitl->wind_direction; float direction = _sitl->wind_direction;
if (direction < 0) { if (direction < 0) {
direction += 360; direction += 360;
} }
control.direction = direction * 100; control.direction = direction * 100;
control.turbulance = _sitl->wind_turbulance * 100; control.turbulance = _sitl->wind_turbulance * 100;
// zero the wind for the first 15s to allow pitot calibration // zero the wind for the first 15s to allow pitot calibration
if (hal.scheduler->millis() < 15000) { if (hal.scheduler->millis() < 15000) {
control.speed = 0; 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 // 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 // generate a random Vector3f of size 1
Vector3f SITL_State::_rand_vec3f(void) Vector3f SITL_State::_rand_vec3f(void)
{ {
Vector3f v = Vector3f(_rand_float(), Vector3f v = Vector3f(_rand_float(),
_rand_float(), _rand_float(),
_rand_float()); _rand_float());
if (v.length() != 0.0f) { if (v.length() != 0.0f) {
v.normalize(); v.normalize();
} }
return v; 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; pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
_scheduler = (SITLScheduler *)hal.scheduler; _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) 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 // remember home altitude as first non-zero altitude
home_alt = _sitl->state.altitude; home_alt = _sitl->state.altitude;
} }
if (_terrain && if (_terrain &&
_sitl->terrain_enable) { _sitl->terrain_enable) {
// get height above terrain from AP_Terrain. This assumes // get height above terrain from AP_Terrain. This assumes
// AP_Terrain is working // AP_Terrain is working
float terrain_height_amsl; float terrain_height_amsl;

36
libraries/AP_HAL_SITL/SITL_State.h
View File

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

124
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

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

52
libraries/AP_HAL_SITL/Scheduler.cpp
View File

@ -34,22 +34,22 @@ SITLScheduler::SITLScheduler(SITL_State *sitlState) :
{ {
} }
void SITLScheduler::init(void *unused) void SITLScheduler::init(void *unused)
{ {
gettimeofday(&_sketch_start_time,NULL); gettimeofday(&_sketch_start_time,NULL);
} }
uint64_t SITLScheduler::_micros64() uint64_t SITLScheduler::_micros64()
{ {
struct timeval tp; struct timeval tp;
gettimeofday(&tp,NULL); gettimeofday(&tp,NULL);
uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec + (_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6))); (_sketch_start_time.tv_usec*1.0e-6)));
return ret; return ret;
} }
uint64_t SITLScheduler::micros64() uint64_t SITLScheduler::micros64()
{ {
if (stopped_clock_usec) { if (stopped_clock_usec) {
return stopped_clock_usec; return stopped_clock_usec;
@ -57,40 +57,40 @@ uint64_t SITLScheduler::micros64()
return _micros64(); return _micros64();
} }
uint32_t SITLScheduler::micros() uint32_t SITLScheduler::micros()
{ {
return micros64() & 0xFFFFFFFF; return micros64() & 0xFFFFFFFF;
} }
uint64_t SITLScheduler::millis64() uint64_t SITLScheduler::millis64()
{ {
if (stopped_clock_usec) { if (stopped_clock_usec) {
return stopped_clock_usec/1000; return stopped_clock_usec/1000;
} }
struct timeval tp; struct timeval tp;
gettimeofday(&tp,NULL); gettimeofday(&tp,NULL);
uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec + (_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6))); (_sketch_start_time.tv_usec*1.0e-6)));
return ret; return ret;
} }
uint32_t SITLScheduler::millis() uint32_t SITLScheduler::millis()
{ {
return millis64() & 0xFFFFFFFF; return millis64() & 0xFFFFFFFF;
} }
void SITLScheduler::delay_microseconds(uint16_t usec) void SITLScheduler::delay_microseconds(uint16_t usec)
{ {
uint64_t start = micros64(); uint64_t start = micros64();
uint64_t dtime; uint64_t dtime;
while ((dtime=(micros64() - start) < usec)) { while ((dtime=(micros64() - start) < usec)) {
if (stopped_clock_usec) { if (stopped_clock_usec) {
_sitlState->wait_clock(start+usec); _sitlState->wait_clock(start+usec);
} else { } else {
usleep(usec - dtime); usleep(usec - dtime);
} }
} }
} }
void SITLScheduler::delay(uint16_t ms) void SITLScheduler::delay(uint16_t ms)
@ -107,13 +107,13 @@ void SITLScheduler::delay(uint16_t ms)
} }
void SITLScheduler::register_delay_callback(AP_HAL::Proc proc, void SITLScheduler::register_delay_callback(AP_HAL::Proc proc,
uint16_t min_time_ms) uint16_t min_time_ms)
{ {
_delay_cb = proc; _delay_cb = proc;
_min_delay_cb_ms = min_time_ms; _min_delay_cb_ms = min_time_ms;
} }
void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc) void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc)
{ {
for (uint8_t i = 0; i < _num_timer_procs; i++) { for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) { if (_timer_proc[i] == proc) {
@ -128,7 +128,7 @@ void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc)
} }
void SITLScheduler::register_io_process(AP_HAL::MemberProc proc) void SITLScheduler::register_io_process(AP_HAL::MemberProc proc)
{ {
for (uint8_t i = 0; i < _num_io_procs; i++) { for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) { if (_io_proc[i] == proc) {
@ -143,7 +143,7 @@ void SITLScheduler::register_io_process(AP_HAL::MemberProc proc)
} }
void SITLScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) void SITLScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
{ {
_failsafe = failsafe; _failsafe = failsafe;
} }
@ -187,18 +187,18 @@ void SITLScheduler::system_initialized() {
} }
void SITLScheduler::sitl_end_atomic() { void SITLScheduler::sitl_end_atomic() {
if (_nested_atomic_ctr == 0) if (_nested_atomic_ctr == 0)
hal.uartA->println_P(PSTR("NESTED ATOMIC ERROR")); hal.uartA->println_P(PSTR("NESTED ATOMIC ERROR"));
else else
_nested_atomic_ctr--; _nested_atomic_ctr--;
} }
void SITLScheduler::reboot(bool hold_in_bootloader) void SITLScheduler::reboot(bool hold_in_bootloader)
{ {
hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n")); hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n"));
} }
void SITLScheduler::_run_timer_procs(bool called_from_isr) void SITLScheduler::_run_timer_procs(bool called_from_isr)
{ {
if (_in_timer_proc) { if (_in_timer_proc) {
// the timer calls took longer than the period of the // the timer calls took longer than the period of the
@ -237,7 +237,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
_in_timer_proc = false; _in_timer_proc = false;
} }
void SITLScheduler::_run_io_procs(bool called_from_isr) void SITLScheduler::_run_io_procs(bool called_from_isr)
{ {
if (_in_io_proc) { if (_in_io_proc) {
return; return;

13
libraries/AP_HAL_SITL/Scheduler.h
View File

@ -39,14 +39,21 @@ public:
void reboot(bool hold_in_bootloader); void reboot(bool hold_in_bootloader);
void panic(const prog_char_t *errormsg); 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(); void sitl_end_atomic();
// callable from interrupt handler // callable from interrupt handler
static uint64_t _micros64(); 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: private:
SITL_State *_sitlState; SITL_State *_sitlState;

24
libraries/AP_HAL_SITL/Storage.cpp
View File

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

2
libraries/AP_HAL_SITL/Storage.h
View File

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

192
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -30,7 +30,7 @@
#include <errno.h> #include <errno.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <netinet/tcp.h> #include <netinet/tcp.h>
@ -45,7 +45,7 @@ bool SITLUARTDriver::_console;
/* UARTDriver method implementations */ /* UARTDriver method implementations */
void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
{ {
if (txSpace != 0) { if (txSpace != 0) {
_txSpace = txSpace; _txSpace = txSpace;
@ -69,25 +69,25 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
_connected = true; _connected = true;
_fd = _sitlState->gps2_pipe(); _fd = _sitlState->gps2_pipe();
break; break;
default: default:
_tcp_start_connection(false); _tcp_start_connection(false);
break; break;
} }
} }
void SITLUARTDriver::end() void SITLUARTDriver::end()
{ {
} }
int16_t SITLUARTDriver::available(void) int16_t SITLUARTDriver::available(void)
{ {
_check_connection(); _check_connection();
if (!_connected) { if (!_connected) {
return 0; return 0;
} }
if (_select_check(_fd)) { if (_select_check(_fd)) {
#ifdef FIONREAD #ifdef FIONREAD
// use FIONREAD to get exact value if possible // use FIONREAD to get exact value if possible
@ -113,13 +113,13 @@ int16_t SITLUARTDriver::available(void)
int16_t SITLUARTDriver::txspace(void) int16_t SITLUARTDriver::txspace(void)
{ {
// always claim there is space available // always claim there is space available
return _txSpace; return _txSpace;
} }
int16_t SITLUARTDriver::read(void) int16_t SITLUARTDriver::read(void)
{ {
char c; char c;
@ -153,11 +153,11 @@ int16_t SITLUARTDriver::read(void)
return -1; return -1;
} }
void SITLUARTDriver::flush(void) void SITLUARTDriver::flush(void)
{ {
} }
size_t SITLUARTDriver::write(uint8_t c) size_t SITLUARTDriver::write(uint8_t c)
{ {
int flags = 0; int flags = 0;
_check_connection(); _check_connection();
@ -188,79 +188,79 @@ size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size)
*/ */
void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection) void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
{ {
int one=1; int one=1;
struct sockaddr_in sockaddr; struct sockaddr_in sockaddr;
int ret; int ret;
if (_connected) { if (_connected) {
return; return;
} }
if (_console) { if (_console) {
// hack for console access // hack for console access
_connected = true; _connected = true;
_listen_fd = -1; _listen_fd = -1;
_fd = 1; _fd = 1;
return; return;
} }
if (_fd != -1) { if (_fd != -1) {
close(_fd); close(_fd);
} }
if (_listen_fd == -1) { if (_listen_fd == -1) {
memset(&sockaddr,0,sizeof(sockaddr)); memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN #ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr); sockaddr.sin_len = sizeof(sockaddr);
#endif #endif
sockaddr.sin_port = htons(_sitlState->base_port() + _portNumber); sockaddr.sin_port = htons(_sitlState->base_port() + _portNumber);
sockaddr.sin_family = AF_INET; sockaddr.sin_family = AF_INET;
_listen_fd = socket(AF_INET, SOCK_STREAM, 0); _listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_listen_fd == -1) { if (_listen_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno)); fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1); exit(1);
} }
/* we want to be able to re-use ports quickly */ /* we want to be able to re-use ports quickly */
setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); 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)ntohs(sockaddr.sin_port),
(unsigned)_portNumber), strerror(errno));
exit(1);
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);
} }
if (wait_for_connection) { ret = listen(_listen_fd, 5);
fprintf(stdout, "Waiting for connection ....\n"); if (ret == -1) {
fflush(stdout); fprintf(stderr, "listen failed - %s\n", strerror(errno));
_fd = accept(_listen_fd, NULL, NULL); exit(1);
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;
} }
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) void SITLUARTDriver::_check_connection(void)
{ {
if (_connected) { if (_connected) {
// we only want 1 connection at a time // we only want 1 connection at a time
return; return;
} }
if (_select_check(_listen_fd)) { if (_select_check(_listen_fd)) {
_fd = accept(_listen_fd, NULL, NULL); _fd = accept(_listen_fd, NULL, NULL);
if (_fd != -1) { if (_fd != -1) {
int one = 1; int one = 1;
_connected = true; _connected = true;
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
fprintf(stdout, "New connection on serial port %u\n", _portNumber); 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) bool SITLUARTDriver::_select_check(int fd)
{ {
fd_set fds; fd_set fds;
struct timeval tv; struct timeval tv;
FD_ZERO(&fds); FD_ZERO(&fds);
FD_SET(fd, &fds); FD_SET(fd, &fds);
// zero time means immediate return from select() // zero time means immediate return from select()
tv.tv_sec = 0; tv.tv_sec = 0;
tv.tv_usec = 0; tv.tv_usec = 0;
if (select(fd+1, &fds, NULL, NULL, &tv) == 1) { if (select(fd+1, &fds, NULL, NULL, &tv) == 1) {
return true; return true;
} }
return false; return false;
} }
void SITLUARTDriver::_set_nonblocking(int fd) void SITLUARTDriver::_set_nonblocking(int fd)
{ {
unsigned v = fcntl(fd, F_GETFL, 0); unsigned v = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, v | O_NONBLOCK); fcntl(fd, F_SETFL, v | O_NONBLOCK);
} }
#endif #endif

52
libraries/AP_HAL_SITL/UARTDriver.h
View File

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

4
libraries/AP_HAL_SITL/Util.h
View File

@ -7,7 +7,9 @@
class HALSITL::SITLUtil : public AP_HAL::Util { class HALSITL::SITLUtil : public AP_HAL::Util {
public: 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__ #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 // storing data from sensor to buffer
if (now - last_store_time_baro >= 10) { // store data every 10 ms. if (now - last_store_time_baro >= 10) { // store data every 10 ms.
last_store_time_baro = now; last_store_time_baro = now;
if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer
store_index_baro = 0; store_index_baro = 0;
} }
buffer_baro[store_index_baro].data = sim_alt; // add data to current index 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 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 store_index_baro = store_index_baro + 1; // increment index
} }
// return delayed measurement // return delayed measurement

700
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -36,9 +36,9 @@ static uint8_t gps_delay;
// state of GPS emulation // state of GPS emulation
static struct gps_state { static struct gps_state {
/* pipe emulating UBLOX GPS serial stream */ /* pipe emulating UBLOX GPS serial stream */
int gps_fd, client_fd; int gps_fd, client_fd;
uint32_t last_update; // milliseconds uint32_t last_update; // milliseconds
} gps_state, gps2_state; } 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) ssize_t SITL_State::gps_read(int fd, void *buf, size_t count)
{ {
#ifdef FIONREAD #ifdef FIONREAD
// use FIONREAD to get exact value if possible // use FIONREAD to get exact value if possible
int num_ready; int num_ready;
while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 256) { while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 256) {
// the pipe is filling up - drain it // the pipe is filling up - drain it
uint8_t tmp[128]; uint8_t tmp[128];
if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) { if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) {
break; break;
} }
} }
#endif #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 SITL_State::gps_pipe(void)
{ {
int fd[2]; int fd[2];
if (gps_state.client_fd != 0) { if (gps_state.client_fd != 0) {
return gps_state.client_fd; return gps_state.client_fd;
} }
pipe(fd); pipe(fd);
gps_state.gps_fd = fd[1]; gps_state.gps_fd = fd[1];
gps_state.client_fd = fd[0]; gps_state.client_fd = fd[0];
gps_state.last_update = _scheduler->millis(); gps_state.last_update = _scheduler->millis();
HALSITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd); HALSITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]); HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps_state.client_fd; return gps_state.client_fd;
} }
/* /*
@ -83,17 +83,17 @@ int SITL_State::gps_pipe(void)
*/ */
int SITL_State::gps2_pipe(void) int SITL_State::gps2_pipe(void)
{ {
int fd[2]; int fd[2];
if (gps2_state.client_fd != 0) { if (gps2_state.client_fd != 0) {
return gps2_state.client_fd; return gps2_state.client_fd;
} }
pipe(fd); pipe(fd);
gps2_state.gps_fd = fd[1]; gps2_state.gps_fd = fd[1];
gps2_state.client_fd = fd[0]; gps2_state.client_fd = fd[0];
gps2_state.last_update = _scheduler->millis(); gps2_state.last_update = _scheduler->millis();
HALSITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd); HALSITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd);
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]); HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
return gps2_state.client_fd; 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) void SITL_State::_gps_write(const uint8_t *p, uint16_t size)
{ {
while (size--) { while (size--) {
if (_sitl->gps_byteloss > 0.0f) { if (_sitl->gps_byteloss > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4; float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < _sitl->gps_byteloss) { if (r < _sitl->gps_byteloss) {
// lose the byte // lose the byte
p++; p++;
continue; continue;
} }
} }
write(gps_state.gps_fd, p, 1); write(gps_state.gps_fd, p, 1);
if (_sitl->gps2_enable) { if (_sitl->gps2_enable) {
write(gps2_state.gps_fd, p, 1); write(gps2_state.gps_fd, p, 1);
} }
p++; 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) void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
{ {
const uint8_t PREAMBLE1 = 0xb5; const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62; const uint8_t PREAMBLE2 = 0x62;
const uint8_t CLASS_NAV = 0x1; const uint8_t CLASS_NAV = 0x1;
uint8_t hdr[6], chk[2]; uint8_t hdr[6], chk[2];
hdr[0] = PREAMBLE1; hdr[0] = PREAMBLE1;
hdr[1] = PREAMBLE2; hdr[1] = PREAMBLE2;
hdr[2] = CLASS_NAV; hdr[2] = CLASS_NAV;
hdr[3] = msgid; hdr[3] = msgid;
hdr[4] = size & 0xFF; hdr[4] = size & 0xFF;
hdr[5] = size >> 8; hdr[5] = size >> 8;
chk[0] = chk[1] = hdr[2]; chk[0] = chk[1] = hdr[2];
chk[1] += (chk[0] += hdr[3]); chk[1] += (chk[0] += hdr[3]);
chk[1] += (chk[0] += hdr[4]); chk[1] += (chk[0] += hdr[4]);
chk[1] += (chk[0] += hdr[5]); chk[1] += (chk[0] += hdr[5]);
for (uint8_t i=0; i<size; i++) { for (uint8_t i=0; i<size; i++) {
chk[1] += (chk[0] += buf[i]); chk[1] += (chk[0] += buf[i]);
} }
_gps_write(hdr, sizeof(hdr)); _gps_write(hdr, sizeof(hdr));
_gps_write(buf, size); _gps_write(buf, size);
_gps_write(chk, sizeof(chk)); _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) void SITL_State::_update_gps_ubx(const struct gps_data *d)
{ {
struct PACKED ubx_nav_posllh { struct PACKED ubx_nav_posllh {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
int32_t longitude; int32_t longitude;
int32_t latitude; int32_t latitude;
int32_t altitude_ellipsoid; int32_t altitude_ellipsoid;
int32_t altitude_msl; int32_t altitude_msl;
uint32_t horizontal_accuracy; uint32_t horizontal_accuracy;
uint32_t vertical_accuracy; uint32_t vertical_accuracy;
} pos; } pos;
struct PACKED ubx_nav_status { struct PACKED ubx_nav_status {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
uint8_t fix_type; uint8_t fix_type;
uint8_t fix_status; uint8_t fix_status;
uint8_t differential_status; uint8_t differential_status;
uint8_t res; uint8_t res;
uint32_t time_to_first_fix; uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds uint32_t uptime; // milliseconds
} status; } status;
struct PACKED ubx_nav_velned { struct PACKED ubx_nav_velned {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
int32_t ned_north; int32_t ned_north;
int32_t ned_east; int32_t ned_east;
int32_t ned_down; int32_t ned_down;
uint32_t speed_3d; uint32_t speed_3d;
uint32_t speed_2d; uint32_t speed_2d;
int32_t heading_2d; int32_t heading_2d;
uint32_t speed_accuracy; uint32_t speed_accuracy;
uint32_t heading_accuracy; uint32_t heading_accuracy;
} velned; } velned;
struct PACKED ubx_nav_solution { struct PACKED ubx_nav_solution {
uint32_t time; uint32_t time;
int32_t time_nsec; int32_t time_nsec;
int16_t week; int16_t week;
uint8_t fix_type; uint8_t fix_type;
uint8_t fix_status; uint8_t fix_status;
int32_t ecef_x; int32_t ecef_x;
int32_t ecef_y; int32_t ecef_y;
int32_t ecef_z; int32_t ecef_z;
uint32_t position_accuracy_3d; uint32_t position_accuracy_3d;
int32_t ecef_x_velocity; int32_t ecef_x_velocity;
int32_t ecef_y_velocity; int32_t ecef_y_velocity;
int32_t ecef_z_velocity; int32_t ecef_z_velocity;
uint32_t speed_accuracy; uint32_t speed_accuracy;
uint16_t position_DOP; uint16_t position_DOP;
uint8_t res; uint8_t res;
uint8_t satellites; uint8_t satellites;
uint32_t res2; uint32_t res2;
} sol; } sol;
const uint8_t MSG_POSLLH = 0x2; const uint8_t MSG_POSLLH = 0x2;
const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_STATUS = 0x3;
const uint8_t MSG_VELNED = 0x12; const uint8_t MSG_VELNED = 0x12;
const uint8_t MSG_SOL = 0x6; const uint8_t MSG_SOL = 0x6;
uint16_t time_week; uint16_t time_week;
uint32_t time_week_ms; uint32_t time_week_ms;
gps_time(&time_week, &time_week_ms); gps_time(&time_week, &time_week_ms);
pos.time = time_week_ms; pos.time = time_week_ms;
pos.longitude = d->longitude * 1.0e7; pos.longitude = d->longitude * 1.0e7;
pos.latitude = d->latitude * 1.0e7; pos.latitude = d->latitude * 1.0e7;
pos.altitude_ellipsoid = d->altitude*1000.0f; pos.altitude_ellipsoid = d->altitude*1000.0f;
pos.altitude_msl = d->altitude*1000.0f; pos.altitude_msl = d->altitude*1000.0f;
pos.horizontal_accuracy = 1500; pos.horizontal_accuracy = 1500;
pos.vertical_accuracy = 2000; pos.vertical_accuracy = 2000;
status.time = time_week_ms; status.time = time_week_ms;
status.fix_type = d->have_lock?3:0; status.fix_type = d->have_lock?3:0;
status.fix_status = d->have_lock?1:0; status.fix_status = d->have_lock?1:0;
status.differential_status = 0; status.differential_status = 0;
status.res = 0; status.res = 0;
status.time_to_first_fix = 0; status.time_to_first_fix = 0;
status.uptime = hal.scheduler->millis(); status.uptime = hal.scheduler->millis();
velned.time = time_week_ms; velned.time = time_week_ms;
velned.ned_north = 100.0f * d->speedN; velned.ned_north = 100.0f * d->speedN;
velned.ned_east = 100.0f * d->speedE; velned.ned_east = 100.0f * d->speedE;
velned.ned_down = 100.0f * d->speedD; velned.ned_down = 100.0f * d->speedD;
velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100; velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100;
velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100; velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100;
velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f;
if (velned.heading_2d < 0.0f) { if (velned.heading_2d < 0.0f) {
velned.heading_2d += 360.0f * 100000.0f; velned.heading_2d += 360.0f * 100000.0f;
} }
velned.speed_accuracy = 40; velned.speed_accuracy = 40;
velned.heading_accuracy = 4; velned.heading_accuracy = 4;
memset(&sol, 0, sizeof(sol)); memset(&sol, 0, sizeof(sol));
sol.fix_type = d->have_lock?3:0; sol.fix_type = d->have_lock?3:0;
sol.fix_status = 221; sol.fix_status = 221;
sol.satellites = d->have_lock?_sitl->gps_numsats:3; sol.satellites = d->have_lock?_sitl->gps_numsats:3;
sol.time = time_week_ms; sol.time = time_week_ms;
sol.week = time_week; sol.week = time_week;
_gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); _gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
_gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); _gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
_gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); _gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); _gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
} }
static void swap_uint32(uint32_t *v, uint8_t n) static void swap_uint32(uint32_t *v, uint8_t n)
{ {
while (n--) { while (n--) {
*v = htonl(*v); *v = htonl(*v);
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) static void mtk_checksum(const uint8_t *data, uint8_t n, uint8_t *ck_a, uint8_t *ck_b)
{ {
*ck_a = *ck_b = 0; *ck_a = *ck_b = 0;
while (n--) { while (n--) {
*ck_a += *data++; *ck_a += *data++;
*ck_b += *ck_a; *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) void SITL_State::_update_gps_mtk(const struct gps_data *d)
{ {
struct PACKED mtk_msg { struct PACKED mtk_msg {
uint8_t preamble1; uint8_t preamble1;
uint8_t preamble2; uint8_t preamble2;
uint8_t msg_class; uint8_t msg_class;
uint8_t msg_id; uint8_t msg_id;
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t altitude; int32_t altitude;
int32_t ground_speed; int32_t ground_speed;
int32_t ground_course; int32_t ground_course;
uint8_t satellites; uint8_t satellites;
uint8_t fix_type; uint8_t fix_type;
uint32_t utc_time; uint32_t utc_time;
uint8_t ck_a; uint8_t ck_a;
uint8_t ck_b; uint8_t ck_b;
} p; } p;
p.preamble1 = 0xb5; p.preamble1 = 0xb5;
p.preamble2 = 0x62; p.preamble2 = 0x62;
p.msg_class = 1; p.msg_class = 1;
p.msg_id = 5; p.msg_id = 5;
p.latitude = d->latitude * 1.0e6; p.latitude = d->latitude * 1.0e6;
p.longitude = d->longitude * 1.0e6; p.longitude = d->longitude * 1.0e6;
p.altitude = d->altitude * 100; p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0f; p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0f;
if (p.ground_course < 0.0f) { if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 1000000.0f; p.ground_course += 360.0f * 1000000.0f;
} }
p.satellites = d->have_lock?_sitl->gps_numsats:3; p.satellites = d->have_lock?_sitl->gps_numsats:3;
p.fix_type = d->have_lock?3:1; p.fix_type = d->have_lock?3:1;
// the spec is not very clear, but the time field seems to be // the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time, // milliseconds since the start of the day in UTC time,
// done in powers of 100. // done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000 // The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm; struct tm tm;
struct timeval tv; struct timeval tv;
gettimeofday(&tv, NULL); gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec); tm = *gmtime(&tv.tv_sec);
uint32_t hsec = (tv.tv_usec / (10000*20)) * 20; // always multiple of 20 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; 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.latitude, 5);
swap_uint32((uint32_t *)&p.utc_time, 1); 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) void SITL_State::_update_gps_mtk16(const struct gps_data *d)
{ {
struct PACKED mtk_msg { struct PACKED mtk_msg {
uint8_t preamble1; uint8_t preamble1;
uint8_t preamble2; uint8_t preamble2;
uint8_t size; uint8_t size;
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t altitude; 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_date;
uint32_t utc_time; uint32_t utc_time;
uint16_t hdop; uint16_t hdop;
uint8_t ck_a; uint8_t ck_a;
uint8_t ck_b; uint8_t ck_b;
} p; } p;
p.preamble1 = 0xd0; p.preamble1 = 0xd0;
p.preamble2 = 0xdd; p.preamble2 = 0xdd;
p.size = sizeof(p) - 5; p.size = sizeof(p) - 5;
p.latitude = d->latitude * 1.0e6; p.latitude = d->latitude * 1.0e6;
p.longitude = d->longitude * 1.0e6; p.longitude = d->longitude * 1.0e6;
p.altitude = d->altitude * 100; p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f;
if (p.ground_course < 0.0f) { if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 100.0f; p.ground_course += 360.0f * 100.0f;
} }
p.satellites = d->have_lock?_sitl->gps_numsats:3; p.satellites = d->have_lock?_sitl->gps_numsats:3;
p.fix_type = d->have_lock?3:1; p.fix_type = d->have_lock?3:1;
// the spec is not very clear, but the time field seems to be // the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time, // milliseconds since the start of the day in UTC time,
// done in powers of 100. // done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000 // The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm; struct tm tm;
struct timeval tv; struct timeval tv;
gettimeofday(&tv, NULL); gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec); tm = *gmtime(&tv.tv_sec);
uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200 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_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.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) void SITL_State::_update_gps_mtk19(const struct gps_data *d)
{ {
struct PACKED mtk_msg { struct PACKED mtk_msg {
uint8_t preamble1; uint8_t preamble1;
uint8_t preamble2; uint8_t preamble2;
uint8_t size; uint8_t size;
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t altitude; 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_date;
uint32_t utc_time; uint32_t utc_time;
uint16_t hdop; uint16_t hdop;
uint8_t ck_a; uint8_t ck_a;
uint8_t ck_b; uint8_t ck_b;
} p; } p;
p.preamble1 = 0xd1; p.preamble1 = 0xd1;
p.preamble2 = 0xdd; p.preamble2 = 0xdd;
p.size = sizeof(p) - 5; p.size = sizeof(p) - 5;
p.latitude = d->latitude * 1.0e7; p.latitude = d->latitude * 1.0e7;
p.longitude = d->longitude * 1.0e7; p.longitude = d->longitude * 1.0e7;
p.altitude = d->altitude * 100; p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f;
if (p.ground_course < 0.0f) { if (p.ground_course < 0.0f) {
p.ground_course += 360.0f * 100.0f; p.ground_course += 360.0f * 100.0f;
} }
p.satellites = d->have_lock?_sitl->gps_numsats:3; p.satellites = d->have_lock?_sitl->gps_numsats:3;
p.fix_type = d->have_lock?3:1; p.fix_type = d->have_lock?3:1;
// the spec is not very clear, but the time field seems to be // the spec is not very clear, but the time field seems to be
// milliseconds since the start of the day in UTC time, // milliseconds since the start of the day in UTC time,
// done in powers of 100. // done in powers of 100.
// The date is powers of 100 as well, but in days since 1/1/2000 // The date is powers of 100 as well, but in days since 1/1/2000
struct tm tm; struct tm tm;
struct timeval tv; struct timeval tv;
gettimeofday(&tv, NULL); gettimeofday(&tv, NULL);
tm = *gmtime(&tv.tv_sec); tm = *gmtime(&tv.tv_sec);
uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200 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_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.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));
} }
/* /*
@ -472,7 +472,7 @@ uint16_t SITL_State::_gps_nmea_checksum(const char *s)
/* /*
formated print of NMEA message, with checksum appended formated print of NMEA message, with checksum appended
*/ */
void SITL_State::_gps_nmea_printf(const char *fmt, ...) void SITL_State::_gps_nmea_printf(const char *fmt, ...)
{ {
char *s = NULL; char *s = NULL;
uint16_t csum; uint16_t csum;
@ -516,24 +516,24 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d)
// format latitude // format latitude
double deg = fabs(d->latitude); double deg = fabs(d->latitude);
snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c",
(unsigned)deg, (unsigned)deg,
(deg - int(deg))*60, (deg - int(deg))*60,
d->latitude<0?'S':'N'); d->latitude<0?'S':'N');
// format longitude // format longitude
deg = fabs(d->longitude); deg = fabs(d->longitude);
snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c",
(unsigned)deg, (unsigned)deg,
(deg - int(deg))*60, (deg - int(deg))*60,
d->longitude<0?'W':'E'); d->longitude<0?'W':'E');
_gps_nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", _gps_nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
tstring, tstring,
lat_string, lat_string,
lng_string, lng_string,
d->have_lock?1:0, d->have_lock?1:0,
d->have_lock?_sitl->gps_numsats:3, d->have_lock?_sitl->gps_numsats:3,
2.0, 2.0,
d->altitude); d->altitude);
float speed_knots = pythagorous2(d->speedN, d->speedE)*1.94384449f; float speed_knots = pythagorous2(d->speedN, d->speedE)*1.94384449f;
float heading = ToDeg(atan2f(d->speedE, d->speedN)); float heading = ToDeg(atan2f(d->speedE, d->speedN));
@ -541,8 +541,8 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d)
heading += 360.0f; heading += 360.0f;
} }
_gps_nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", _gps_nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
tstring, tstring,
d->have_lock?'A':'V', d->have_lock?'A':'V',
lat_string, lat_string,
lng_string, lng_string,
speed_knots, speed_knots,
@ -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) 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) { if (len != 0 && payload == 0) {
return; //SBP_NULL_ERROR; return; //SBP_NULL_ERROR;
} }
uint8_t preamble = 0x55; uint8_t preamble = 0x55;
_gps_write(&preamble, 1); _gps_write(&preamble, 1);
_gps_write((uint8_t*)&msg_type, 2); _gps_write((uint8_t*)&msg_type, 2);
_gps_write((uint8_t*)&sender_id, 2); _gps_write((uint8_t*)&sender_id, 2);
_gps_write(&len, 1); _gps_write(&len, 1);
if (len > 0) { if (len > 0) {
_gps_write((uint8_t*)payload, len); _gps_write((uint8_t*)payload, len);
} }
uint16_t crc; uint16_t crc;
crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0); crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc); crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc); crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc); crc = crc16_ccitt(payload, len, crc);
_gps_write((uint8_t*)&crc, 2); _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); _sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) { if (!d->have_lock) {
return; return;
} }
pos.tow = time_week_ms; pos.tow = time_week_ms;
@ -660,20 +660,20 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d)
static uint32_t do_every_count = 0; static uint32_t do_every_count = 0;
if (do_every_count % 5 == 0) { if (do_every_count % 5 == 0) {
dops.tow = time_week_ms; dops.tow = time_week_ms;
dops.gdop = 1; dops.gdop = 1;
dops.pdop = 1; dops.pdop = 1;
dops.tdop = 1; dops.tdop = 1;
dops.hdop = 100; dops.hdop = 100;
dops.vdop = 1; dops.vdop = 1;
_sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), _sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&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++; do_every_count++;
} }
@ -682,106 +682,106 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d)
possibly send a new GPS packet possibly send a new GPS packet
*/ */
void SITL_State::_update_gps(double latitude, double longitude, float altitude, 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; struct gps_data d;
char c; char c;
Vector3f glitch_offsets = _sitl->gps_glitch; Vector3f glitch_offsets = _sitl->gps_glitch;
//Capture current position as basestation location for //Capture current position as basestation location for
if (!_gps_has_basestation_position) { if (!_gps_has_basestation_position) {
if (have_lock) { if (have_lock) {
_gps_basestation_data.latitude = latitude; _gps_basestation_data.latitude = latitude;
_gps_basestation_data.longitude = longitude; _gps_basestation_data.longitude = longitude;
_gps_basestation_data.altitude = altitude; _gps_basestation_data.altitude = altitude;
_gps_basestation_data.speedN = speedN; _gps_basestation_data.speedN = speedN;
_gps_basestation_data.speedE = speedE; _gps_basestation_data.speedE = speedE;
_gps_basestation_data.speedD = speedD; _gps_basestation_data.speedD = speedD;
_gps_basestation_data.have_lock = have_lock; _gps_basestation_data.have_lock = have_lock;
_gps_has_basestation_position = true; _gps_has_basestation_position = true;
} }
} }
// run at configured GPS rate (default 5Hz) // run at configured GPS rate (default 5Hz)
if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) { if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
return; return;
} }
// swallow any config bytes // swallow any config bytes
if (gps_state.gps_fd != 0) { if (gps_state.gps_fd != 0) {
read(gps_state.gps_fd, &c, 1); read(gps_state.gps_fd, &c, 1);
} }
if (gps2_state.gps_fd != 0) { if (gps2_state.gps_fd != 0) {
read(gps2_state.gps_fd, &c, 1); read(gps2_state.gps_fd, &c, 1);
} }
gps_state.last_update = hal.scheduler->millis(); gps_state.last_update = hal.scheduler->millis();
gps2_state.last_update = hal.scheduler->millis(); gps2_state.last_update = hal.scheduler->millis();
d.latitude = latitude + glitch_offsets.x; d.latitude = latitude + glitch_offsets.x;
d.longitude = longitude + glitch_offsets.y; d.longitude = longitude + glitch_offsets.y;
d.altitude = altitude + glitch_offsets.z; d.altitude = altitude + glitch_offsets.z;
if (_sitl->gps_drift_alt > 0) { if (_sitl->gps_drift_alt > 0) {
// slow altitude drift // slow altitude drift
d.altitude += _sitl->gps_drift_alt*sinf(hal.scheduler->millis()*0.001f*0.02f); 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;
// add in some GPS lag d.speedN = speedN;
_gps_data[next_gps_index++] = d; d.speedE = speedE;
if (next_gps_index >= gps_delay+1) { d.speedD = speedD;
next_gps_index = 0; d.have_lock = have_lock;
}
d = _gps_data[next_gps_index]; // add in some GPS lag
_gps_data[next_gps_index++] = d;
if (next_gps_index >= gps_delay+1) {
next_gps_index = 0;
}
if (_sitl->gps_delay != gps_delay) { d = _gps_data[next_gps_index];
// 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) { if (_sitl->gps_delay != gps_delay) {
return; // 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;
}
}
switch ((SITL::GPSType)_sitl->gps_type.get()) { if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) {
case SITL::GPS_TYPE_NONE: return;
// no GPS attached }
break;
case SITL::GPS_TYPE_UBLOX: switch ((SITL::GPSType)_sitl->gps_type.get()) {
_update_gps_ubx(&d); case SITL::GPS_TYPE_NONE:
break; // no GPS attached
break;
case SITL::GPS_TYPE_MTK: case SITL::GPS_TYPE_UBLOX:
_update_gps_mtk(&d); _update_gps_ubx(&d);
break; break;
case SITL::GPS_TYPE_MTK16: case SITL::GPS_TYPE_MTK:
_update_gps_mtk16(&d); _update_gps_mtk(&d);
break; break;
case SITL::GPS_TYPE_MTK19: case SITL::GPS_TYPE_MTK16:
_update_gps_mtk19(&d); _update_gps_mtk16(&d);
break; break;
case SITL::GPS_TYPE_NMEA: case SITL::GPS_TYPE_MTK19:
_update_gps_nmea(&d); _update_gps_mtk19(&d);
break; break;
case SITL::GPS_TYPE_SBP: case SITL::GPS_TYPE_NMEA:
_update_gps_sbp(&d); _update_gps_nmea(&d);
break; break;
} case SITL::GPS_TYPE_SBP:
_update_gps_sbp(&d);
break;
}
} }
#endif #endif

142
libraries/AP_HAL_SITL/sitl_ins.cpp
View File

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

12
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; static uint32_t last_flow_ms;
if (!_optical_flow || if (!_optical_flow ||
!_sitl->flow_enable) { !_sitl->flow_enable) {
return; return;
} }
@ -52,8 +52,8 @@ void SITL_State::_update_flow(void)
// convert roll rates to body frame // convert roll rates to body frame
SITL::convert_body_frame(_sitl->state.rollDeg, SITL::convert_body_frame(_sitl->state.rollDeg,
_sitl->state.pitchDeg, _sitl->state.pitchDeg,
_sitl->state.rollRate, _sitl->state.rollRate,
_sitl->state.pitchRate, _sitl->state.pitchRate,
_sitl->state.yawRate, _sitl->state.yawRate,
&p, &q, &r); &p, &q, &r);
gyro(p, q, r); gyro(p, q, r);
@ -61,8 +61,8 @@ void SITL_State::_update_flow(void)
OpticalFlow::OpticalFlow_state state; OpticalFlow::OpticalFlow_state state;
// NED velocity vector in m/s // NED velocity vector in m/s
Vector3f velocity(_sitl->state.speedN, Vector3f velocity(_sitl->state.speedN,
_sitl->state.speedE, _sitl->state.speedE,
_sitl->state.speedD); _sitl->state.speedD);
// a rotation matrix following DCM conventions // a rotation matrix following DCM conventions
@ -70,7 +70,7 @@ void SITL_State::_update_flow(void)
rotmat.from_euler(radians(_sitl->state.rollDeg), rotmat.from_euler(radians(_sitl->state.rollDeg),
radians(_sitl->state.pitchDeg), radians(_sitl->state.pitchDeg),
radians(_sitl->state.yawDeg)); radians(_sitl->state.yawDeg));
state.device_id = 1; state.device_id = 1;
state.surface_quality = 51; state.surface_quality = 51;