diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h index f557aad3de..154394d187 100644 --- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h +++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h @@ -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__ diff --git a/libraries/AP_HAL_SITL/AnalogIn.cpp b/libraries/AP_HAL_SITL/AnalogIn.cpp index d9fc682631..a14f801bdb 100644 --- a/libraries/AP_HAL_SITL/AnalogIn.cpp +++ b/libraries/AP_HAL_SITL/AnalogIn.cpp @@ -17,22 +17,22 @@ 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() { switch (_pin) { case ANALOG_INPUT_BOARD_VCC: return 1023; - + case 0: return _sitlState->sonar_pin_value; @@ -59,7 +59,7 @@ void SITLAnalogIn::init(void *ap_hal_scheduler) { } AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) { - return new ADCSource(_sitlState, pin); + return new ADCSource(_sitlState, pin); } #endif diff --git a/libraries/AP_HAL_SITL/AnalogIn.h b/libraries/AP_HAL_SITL/AnalogIn.h index 0efe30e480..6b87678c2f 100644 --- a/libraries/AP_HAL_SITL/AnalogIn.h +++ b/libraries/AP_HAL_SITL/AnalogIn.h @@ -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) {} @@ -28,16 +30,18 @@ private: 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 */ 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; diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index 3b17708cef..425a42ee4f 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -44,7 +44,7 @@ static SITLUtil utilInstance; HAL_SITL::HAL_SITL() : AP_HAL::HAL( - &sitlUart0Driver, /* uartA */ + &sitlUart0Driver, /* uartA */ &sitlUart1Driver, /* uartB */ &sitlUart2Driver, /* uartC */ &sitlUart3Driver, /* uartD */ @@ -62,7 +62,7 @@ HAL_SITL::HAL_SITL() : _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); scheduler->init(NULL); diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index 59fc532e6b..007ce0a890 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -12,7 +12,7 @@ class HAL_SITL : public AP_HAL::HAL { public: - HAL_SITL(); + HAL_SITL(); void init(int argc, char * const argv[]) const; private: diff --git a/libraries/AP_HAL_SITL/RCInput.cpp b/libraries/AP_HAL_SITL/RCInput.cpp index 2fb673696b..7365d856a6 100644 --- a/libraries/AP_HAL_SITL/RCInput.cpp +++ b/libraries/AP_HAL_SITL/RCInput.cpp @@ -12,7 +12,7 @@ void SITLRCInput::init(void* machtnichts) clear_overrides(); } -bool SITLRCInput::new_input() +bool SITLRCInput::new_input() { if (_sitlState->new_rc_input) { _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) { for (uint8_t i=0; ipwm_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 diff --git a/libraries/AP_HAL_SITL/RCInput.h b/libraries/AP_HAL_SITL/RCInput.h index d7ed75fce6..6b4bd35d76 100644 --- a/libraries/AP_HAL_SITL/RCInput.h +++ b/libraries/AP_HAL_SITL/RCInput.h @@ -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); diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index a8a2f6d9b8..71e6aa2606 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/libraries/AP_HAL_SITL/RCOutput.cpp @@ -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) +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 diff --git a/libraries/AP_HAL_SITL/RCOutput.h b/libraries/AP_HAL_SITL/RCOutput.h index fcbf8d7842..c4835d5c19 100644 --- a/libraries/AP_HAL_SITL/RCOutput.h +++ b/libraries/AP_HAL_SITL/RCOutput.h @@ -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); @@ -22,7 +22,7 @@ public: uint16_t read(uint8_t ch); void read(uint16_t* period_us, uint8_t len); -private: +private: SITL_State *_sitlState; uint16_t _freq_hz; }; diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 2b47537f20..cfe481144d 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -37,7 +37,7 @@ void SITL_State::_set_param_default(const char *parm) AP_Param *vp = AP_Param::find(parm, &var_type); if (vp == NULL) { printf("Unknown parameter %s\n", parm); - exit(1); + exit(1); } if (var_type == AP_PARAM_FLOAT) { ((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) { #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 @@ -150,7 +150,7 @@ void SITL_State::_fdm_input_step(void) } else { tv.tv_sec = 1; tv.tv_usec = 0; - + FD_ZERO(&fds); FD_SET(_sitl_fd, &fds); if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) { @@ -158,7 +158,7 @@ void SITL_State::_fdm_input_step(void) _simulator_output(true); return; } - + /* check for packet from flight sim */ _fdm_input(); } @@ -167,7 +167,7 @@ void SITL_State::_fdm_input_step(void) if (kill(_parent_pid, 0) != 0) { exit(1); } - + if (_scheduler->interrupts_are_blocked() || _sitl == NULL) { return; } @@ -201,8 +201,8 @@ void SITL_State::_fdm_input_step(void) _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg); _update_flow(); } - - // trigger all APM timers. + + // trigger all APM timers. _scheduler->timer_event(); _scheduler->sitl_end_atomic(); } @@ -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; diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index fb14ff33af..b8dff9687a 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -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); @@ -112,7 +114,7 @@ private: float _rand_float(void); Vector3f _rand_vec3f(void); void _fdm_input_step(void); - + void wait_clock(uint64_t wait_time_usec); pthread_t _fdm_thread_ctx; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 7ef74ce534..2e25443b5a 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -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; iwait_clock(start+usec); } else { usleep(usec - dtime); } - } + } } 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, - uint16_t min_time_ms) + uint16_t min_time_ms) { _delay_cb = proc; _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++) { 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++) { 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; } @@ -187,18 +187,18 @@ void SITLScheduler::system_initialized() { } void SITLScheduler::sitl_end_atomic() { - if (_nested_atomic_ctr == 0) + if (_nested_atomic_ctr == 0) hal.uartA->println_P(PSTR("NESTED ATOMIC ERROR")); else _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")); } -void SITLScheduler::_run_timer_procs(bool called_from_isr) +void SITLScheduler::_run_timer_procs(bool called_from_isr) { if (_in_timer_proc) { // 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; } -void SITLScheduler::_run_io_procs(bool called_from_isr) +void SITLScheduler::_run_io_procs(bool called_from_isr) { if (_in_io_proc) { return; diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index a6fd741248..e46566411b 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -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; diff --git a/libraries/AP_HAL_SITL/Storage.cpp b/libraries/AP_HAL_SITL/Storage.cpp index bcf5168edd..0f936b8ba4 100644 --- a/libraries/AP_HAL_SITL/Storage.cpp +++ b/libraries/AP_HAL_SITL/Storage.cpp @@ -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) +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) +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 diff --git a/libraries/AP_HAL_SITL/Storage.h b/libraries/AP_HAL_SITL/Storage.h index 7e2825623d..74801e3141 100644 --- a/libraries/AP_HAL_SITL/Storage.h +++ b/libraries/AP_HAL_SITL/Storage.h @@ -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); diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 9545f76596..80ae644a3e 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include #include @@ -45,7 +45,7 @@ bool SITLUARTDriver::_console; /* 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) { _txSpace = txSpace; @@ -69,25 +69,25 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) _connected = true; _fd = _sitlState->gps2_pipe(); break; - + default: _tcp_start_connection(false); break; } } -void SITLUARTDriver::end() +void SITLUARTDriver::end() { } -int16_t SITLUARTDriver::available(void) +int16_t SITLUARTDriver::available(void) { _check_connection(); if (!_connected) { return 0; } - + if (_select_check(_fd)) { #ifdef FIONREAD // 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 return _txSpace; } -int16_t SITLUARTDriver::read(void) +int16_t SITLUARTDriver::read(void) { char c; @@ -153,11 +153,11 @@ int16_t SITLUARTDriver::read(void) 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; _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) { - 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 diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index d490374f4a..5081ffc6fe 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -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) + 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; diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index b34c741d91..eedc57921d 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -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__ diff --git a/libraries/AP_HAL_SITL/sitl_barometer.cpp b/libraries/AP_HAL_SITL/sitl_barometer.cpp index f8abd5c600..8fd76f1c56 100644 --- a/libraries/AP_HAL_SITL/sitl_barometer.cpp +++ b/libraries/AP_HAL_SITL/sitl_barometer.cpp @@ -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 diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index c6b34ead2b..7a8f51eb56 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -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> 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; ilongitude * 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)); } /* @@ -472,7 +472,7 @@ uint16_t SITL_State::_gps_nmea_checksum(const char *s) /* 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; uint16_t csum; @@ -516,24 +516,24 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d) // format latitude double deg = fabs(d->latitude); snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", - (unsigned)deg, + (unsigned)deg, (deg - int(deg))*60, d->latitude<0?'S':'N'); // format longitude deg = fabs(d->longitude); snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", - (unsigned)deg, + (unsigned)deg, (deg - int(deg))*60, d->longitude<0?'W':'E'); _gps_nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", - tstring, - lat_string, + tstring, + lat_string, lng_string, - d->have_lock?1:0, + d->have_lock?1:0, d->have_lock?_sitl->gps_numsats:3, - 2.0, + 2.0, d->altitude); float speed_knots = pythagorous2(d->speedN, d->speedE)*1.94384449f; 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; } _gps_nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", - tstring, - d->have_lock?'A':'V', + tstring, + d->have_lock?'A':'V', lat_string, lng_string, 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) { - 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,20 +660,20 @@ 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 + //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; - // add in some GPS lag - _gps_data[next_gps_index++] = d; - if (next_gps_index >= gps_delay+1) { - next_gps_index = 0; - } + d.speedN = speedN; + d.speedE = speedE; + d.speedD = speedD; + 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) { - // cope with updates to the delay control - gps_delay = _sitl->gps_delay; - for (uint8_t i=0; igps_delay != gps_delay) { + // cope with updates to the delay control + gps_delay = _sitl->gps_delay; + for (uint8_t i=0; igps_type.get()) { - case SITL::GPS_TYPE_NONE: - // no GPS attached - break; + if (gps_state.gps_fd == 0 && gps2_state.gps_fd == 0) { + return; + } - case SITL::GPS_TYPE_UBLOX: - _update_gps_ubx(&d); - break; + switch ((SITL::GPSType)_sitl->gps_type.get()) { + case SITL::GPS_TYPE_NONE: + // no GPS attached + break; - case SITL::GPS_TYPE_MTK: - _update_gps_mtk(&d); - break; + case SITL::GPS_TYPE_UBLOX: + _update_gps_ubx(&d); + break; - case SITL::GPS_TYPE_MTK16: - _update_gps_mtk16(&d); - break; + case SITL::GPS_TYPE_MTK: + _update_gps_mtk(&d); + break; - case SITL::GPS_TYPE_MTK19: - _update_gps_mtk19(&d); - break; + case SITL::GPS_TYPE_MTK16: + _update_gps_mtk16(&d); + break; - case SITL::GPS_TYPE_NMEA: - _update_gps_nmea(&d); - break; + case SITL::GPS_TYPE_MTK19: + _update_gps_mtk19(&d); + break; - case SITL::GPS_TYPE_SBP: - _update_gps_sbp(&d); - break; + case SITL::GPS_TYPE_NMEA: + _update_gps_nmea(&d); + break; - } + case SITL::GPS_TYPE_SBP: + _update_gps_sbp(&d); + break; + + } } #endif diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index 659f7c6ef4..a493f783c9 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -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,21 +101,21 @@ 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)); - + altitude += _sitl->sonar_noise * _rand_float(); // Altitude in in m, scaler in meters/volt voltage = altitude / _sitl->sonar_scale; voltage = constrain_float(voltage, 0, 5.0f); - + if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) { 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 - 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 diff --git a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp index 8d132a44ef..3af71ea111 100644 --- a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp +++ b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp @@ -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; } @@ -52,8 +52,8 @@ void SITL_State::_update_flow(void) // convert roll rates to body frame SITL::convert_body_frame(_sitl->state.rollDeg, _sitl->state.pitchDeg, - _sitl->state.rollRate, - _sitl->state.pitchRate, + _sitl->state.rollRate, + _sitl->state.pitchRate, _sitl->state.yawRate, &p, &q, &r); gyro(p, q, r); @@ -61,8 +61,8 @@ void SITL_State::_update_flow(void) OpticalFlow::OpticalFlow_state state; // NED velocity vector in m/s - Vector3f velocity(_sitl->state.speedN, - _sitl->state.speedE, + Vector3f velocity(_sitl->state.speedN, + _sitl->state.speedE, _sitl->state.speedD); // a rotation matrix following DCM conventions @@ -70,7 +70,7 @@ void SITL_State::_update_flow(void) rotmat.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(_sitl->state.yawDeg)); - + state.device_id = 1; state.surface_quality = 51;