2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2020-09-12 16:04:38 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include "AP_HAL_SITL.h"
|
2015-05-04 03:15:12 -03:00
|
|
|
#include "AP_HAL_SITL_Namespace.h"
|
|
|
|
#include "HAL_SITL_Class.h"
|
2012-12-14 21:54:26 -04:00
|
|
|
#include "UARTDriver.h"
|
|
|
|
#include "Scheduler.h"
|
2023-08-17 17:18:36 -03:00
|
|
|
#include "CANSocketIface.h"
|
2012-12-14 21:54:26 -04:00
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <signal.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <errno.h>
|
2012-12-18 03:12:05 -04:00
|
|
|
#include <sys/select.h>
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2016-01-01 02:09:07 -04:00
|
|
|
#include <SITL/SIM_JSBSim.h>
|
2016-01-03 17:22:02 -04:00
|
|
|
#include <AP_HAL/utility/Socket.h>
|
2015-03-21 11:27:25 -03:00
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2015-05-04 03:15:12 -03:00
|
|
|
using namespace HALSITL;
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-05-04 18:08:42 -03:00
|
|
|
void SITL_State::_set_param_default(const char *parm)
|
2014-04-10 23:28:32 -03:00
|
|
|
{
|
2015-05-04 18:08:42 -03:00
|
|
|
char *pdup = strdup(parm);
|
|
|
|
char *p = strchr(pdup, '=');
|
2016-10-30 02:24:21 -03:00
|
|
|
if (p == nullptr) {
|
2014-04-10 23:28:32 -03:00
|
|
|
printf("Please specify parameter as NAME=VALUE");
|
|
|
|
exit(1);
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
float value = strtof(p+1, nullptr);
|
2014-04-10 23:28:32 -03:00
|
|
|
*p = 0;
|
|
|
|
enum ap_var_type var_type;
|
2015-10-01 22:15:21 -03:00
|
|
|
AP_Param *vp = AP_Param::find(pdup, &var_type);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (vp == nullptr) {
|
2015-10-01 22:15:21 -03:00
|
|
|
printf("Unknown parameter %s\n", pdup);
|
2015-05-04 21:59:07 -03:00
|
|
|
exit(1);
|
2014-04-10 23:28:32 -03:00
|
|
|
}
|
|
|
|
if (var_type == AP_PARAM_FLOAT) {
|
|
|
|
((AP_Float *)vp)->set_and_save(value);
|
|
|
|
} else if (var_type == AP_PARAM_INT32) {
|
|
|
|
((AP_Int32 *)vp)->set_and_save(value);
|
|
|
|
} else if (var_type == AP_PARAM_INT16) {
|
|
|
|
((AP_Int16 *)vp)->set_and_save(value);
|
|
|
|
} else if (var_type == AP_PARAM_INT8) {
|
|
|
|
((AP_Int8 *)vp)->set_and_save(value);
|
|
|
|
} else {
|
2015-10-01 22:15:21 -03:00
|
|
|
printf("Unable to set parameter %s\n", pdup);
|
2014-04-10 23:28:32 -03:00
|
|
|
exit(1);
|
|
|
|
}
|
2015-10-01 22:15:21 -03:00
|
|
|
printf("Set parameter %s to %f\n", pdup, value);
|
2015-05-04 18:08:42 -03:00
|
|
|
free(pdup);
|
2014-04-10 23:28:32 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
/*
|
|
|
|
setup for SITL handling
|
|
|
|
*/
|
2021-12-11 05:49:33 -04:00
|
|
|
void SITL_State::_sitl_setup()
|
2012-12-14 21:54:26 -04:00
|
|
|
{
|
2018-03-02 00:28:15 -04:00
|
|
|
#if !defined(__CYGWIN__) && !defined(__CYGWIN64__)
|
2015-05-04 21:59:07 -03:00
|
|
|
_parent_pid = getppid();
|
2012-12-14 21:54:26 -04:00
|
|
|
#endif
|
|
|
|
|
2015-05-04 21:59:07 -03:00
|
|
|
_setup_fdm();
|
|
|
|
fprintf(stdout, "Starting SITL input\n");
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-05-04 21:59:07 -03:00
|
|
|
// find the barometer object if it exists
|
2018-06-15 17:03:18 -03:00
|
|
|
_sitl = AP::sitl();
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_sitl != nullptr) {
|
2012-12-14 21:54:26 -04:00
|
|
|
// setup some initial values
|
2017-05-11 10:38:32 -03:00
|
|
|
_update_airspeed(0);
|
2015-05-24 23:11:16 -03:00
|
|
|
if (enable_gimbal) {
|
2016-02-26 01:15:00 -04:00
|
|
|
gimbal = new SITL::Gimbal(_sitl->state);
|
2015-05-24 23:11:16 -03:00
|
|
|
}
|
2016-01-01 02:09:07 -04:00
|
|
|
|
2019-10-08 01:14:45 -03:00
|
|
|
sitl_model->set_buzzer(&_sitl->buzzer_sim);
|
2018-07-31 09:32:43 -03:00
|
|
|
sitl_model->set_sprayer(&_sitl->sprayer_sim);
|
|
|
|
sitl_model->set_gripper_servo(&_sitl->gripper_sim);
|
|
|
|
sitl_model->set_gripper_epm(&_sitl->gripper_epm_sim);
|
2019-01-08 21:40:22 -04:00
|
|
|
sitl_model->set_parachute(&_sitl->parachute_sim);
|
2019-02-28 22:24:15 -04:00
|
|
|
sitl_model->set_precland(&_sitl->precland_sim);
|
2020-10-20 19:41:31 -03:00
|
|
|
_sitl->i2c_sim.init();
|
2020-08-03 00:24:26 -03:00
|
|
|
sitl_model->set_i2c(&_sitl->i2c_sim);
|
2023-01-22 03:33:55 -04:00
|
|
|
#if AP_TEST_DRONECAN_DRIVERS
|
|
|
|
sitl_model->set_dronecan_device(&_sitl->dronecan_sim);
|
|
|
|
#endif
|
2016-09-21 14:02:15 -03:00
|
|
|
if (_use_fg_view) {
|
2018-06-16 22:39:41 -03:00
|
|
|
fg_socket.connect(_fg_address, _fg_view_port);
|
2016-09-21 14:02:15 -03:00
|
|
|
}
|
|
|
|
|
2016-11-21 09:39:13 -04:00
|
|
|
fprintf(stdout, "Using Irlock at port : %d\n", _irlock_port);
|
2016-11-17 14:05:04 -04:00
|
|
|
_sitl->irlock_port = _irlock_port;
|
2012-12-14 21:54:26 -04:00
|
|
|
}
|
2015-03-21 11:27:25 -03:00
|
|
|
|
|
|
|
if (_synthetic_clock_mode) {
|
|
|
|
// start with non-zero clock
|
2015-04-30 04:15:40 -03:00
|
|
|
hal.scheduler->stop_clock(1);
|
2015-03-21 11:27:25 -03:00
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
setup a SITL FDM listening UDP port
|
|
|
|
*/
|
|
|
|
void SITL_State::_setup_fdm(void)
|
|
|
|
{
|
2018-12-19 06:44:34 -04:00
|
|
|
if (!_sitl_rc_in.reuseaddress()) {
|
|
|
|
fprintf(stderr, "SITL: socket reuseaddress failed on RC in port: %d - %s\n", _rcin_port, strerror(errno));
|
2018-06-08 01:12:58 -03:00
|
|
|
fprintf(stderr, "Aborting launch...\n");
|
2015-05-04 21:59:07 -03:00
|
|
|
exit(1);
|
|
|
|
}
|
2018-12-19 06:44:34 -04:00
|
|
|
if (!_sitl_rc_in.bind("0.0.0.0", _rcin_port)) {
|
|
|
|
fprintf(stderr, "SITL: socket bind failed on RC in port : %d - %s\n", _rcin_port, strerror(errno));
|
2018-12-13 22:37:20 -04:00
|
|
|
fprintf(stderr, "Aborting launch...\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
if (!_sitl_rc_in.set_blocking(false)) {
|
|
|
|
fprintf(stderr, "SITL: socket set_blocking(false) failed on RC in port: %d - %s\n", _rcin_port, strerror(errno));
|
|
|
|
fprintf(stderr, "Aborting launch...\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
if (!_sitl_rc_in.set_cloexec()) {
|
|
|
|
fprintf(stderr, "SITL: socket set_cloexec() failed on RC in port: %d - %s\n", _rcin_port, strerror(errno));
|
|
|
|
fprintf(stderr, "Aborting launch...\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
2015-05-02 08:41:13 -03:00
|
|
|
step the FDM by one time step
|
2012-12-14 21:54:26 -04:00
|
|
|
*/
|
2015-05-02 08:41:13 -03:00
|
|
|
void SITL_State::_fdm_input_step(void)
|
2012-12-14 21:54:26 -04:00
|
|
|
{
|
2015-05-02 08:41:13 -03:00
|
|
|
static uint32_t last_pwm_input = 0;
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-11-22 22:21:33 -04:00
|
|
|
_fdm_input_local();
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-05-02 08:41:13 -03:00
|
|
|
/* make sure we die if our parent dies */
|
|
|
|
if (kill(_parent_pid, 0) != 0) {
|
|
|
|
exit(1);
|
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_scheduler->interrupts_are_blocked() || _sitl == nullptr) {
|
2015-05-02 08:41:13 -03:00
|
|
|
return;
|
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-05-02 08:41:13 -03:00
|
|
|
// simulate RC input at 50Hz
|
2021-08-13 05:07:37 -03:00
|
|
|
if (AP_HAL::millis() - last_pwm_input >= 20 && _sitl != nullptr && _sitl->rc_fail != SITL::SIM::SITL_RCFail_NoPulses) {
|
2015-11-19 23:11:17 -04:00
|
|
|
last_pwm_input = AP_HAL::millis();
|
2015-05-02 08:41:13 -03:00
|
|
|
new_rc_input = true;
|
|
|
|
}
|
2015-03-21 11:27:25 -03:00
|
|
|
|
2015-05-02 08:41:13 -03:00
|
|
|
_scheduler->sitl_begin_atomic();
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_update_count == 0 && _sitl != nullptr) {
|
2021-08-13 05:06:02 -03:00
|
|
|
HALSITL::Scheduler::timer_event();
|
2015-03-21 11:27:25 -03:00
|
|
|
_scheduler->sitl_end_atomic();
|
2015-05-02 08:41:13 -03:00
|
|
|
return;
|
|
|
|
}
|
2015-03-21 11:27:25 -03:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_sitl != nullptr) {
|
2017-05-11 10:38:32 -03:00
|
|
|
_update_airspeed(_sitl->state.airspeed);
|
2021-12-07 21:49:53 -04:00
|
|
|
_update_rangefinder();
|
2015-05-02 08:41:13 -03:00
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
|
|
|
|
// trigger all APM timers.
|
2021-08-13 05:06:02 -03:00
|
|
|
HALSITL::Scheduler::timer_event();
|
2015-05-02 08:41:13 -03:00
|
|
|
_scheduler->sitl_end_atomic();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void SITL_State::wait_clock(uint64_t wait_time_usec)
|
|
|
|
{
|
2022-11-06 19:25:52 -04:00
|
|
|
float speedup = sitl_model->get_speedup();
|
|
|
|
if (speedup < 1) {
|
|
|
|
// for purposes of sleeps treat low speedups as 1
|
|
|
|
speedup = 1.0;
|
|
|
|
}
|
2015-11-19 23:11:17 -04:00
|
|
|
while (AP_HAL::micros64() < wait_time_usec) {
|
2019-09-02 03:00:16 -03:00
|
|
|
if (hal.scheduler->in_main_thread() ||
|
|
|
|
Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
|
2018-07-06 23:42:00 -03:00
|
|
|
_fdm_input_step();
|
|
|
|
} else {
|
2022-11-06 19:25:52 -04:00
|
|
|
#ifdef CYGWIN_BUILD
|
2022-11-08 23:26:34 -04:00
|
|
|
if (speedup > 2 && hal.util->get_soft_armed()) {
|
|
|
|
const char *current_thread = Scheduler::from(hal.scheduler)->get_current_thread_name();
|
|
|
|
if (current_thread && strcmp(current_thread, "Scripting") == 0) {
|
|
|
|
// this effectively does a yield of the CPU. The
|
|
|
|
// granularity of sleeps on cygwin is very high,
|
|
|
|
// so this is needed for good thread performance
|
|
|
|
// in scripting. We don't do this at low speedups
|
|
|
|
// as it causes the cpu to run hot
|
|
|
|
// We also don't do it while disarmed, as lua performance is less
|
|
|
|
// critical while disarmed
|
|
|
|
usleep(0);
|
|
|
|
continue;
|
|
|
|
}
|
2022-11-06 19:25:52 -04:00
|
|
|
}
|
|
|
|
#endif
|
2018-07-06 23:42:00 -03:00
|
|
|
usleep(1000);
|
|
|
|
}
|
2015-03-21 11:27:25 -03:00
|
|
|
}
|
2020-04-02 21:50:25 -03:00
|
|
|
// check the outbound TCP queue size. If it is too long then
|
|
|
|
// MAVProxy/pymavlink take too long to process packets and it ends
|
|
|
|
// up seeing traffic well into our past and hits time-out
|
|
|
|
// conditions.
|
2022-11-06 19:25:52 -04:00
|
|
|
if (speedup > 1 && hal.scheduler->in_main_thread()) {
|
2020-04-02 21:50:25 -03:00
|
|
|
while (true) {
|
2020-12-10 20:18:38 -04:00
|
|
|
const int queue_length = ((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length();
|
2020-04-02 21:50:25 -03:00
|
|
|
// ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length);
|
|
|
|
if (queue_length < 1024) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
usleep(1000);
|
|
|
|
}
|
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2016-09-21 14:15:00 -03:00
|
|
|
check for a SITL RC input packet
|
2012-12-14 21:54:26 -04:00
|
|
|
*/
|
2016-09-21 14:15:00 -03:00
|
|
|
void SITL_State::_check_rc_input(void)
|
2012-12-14 21:54:26 -04:00
|
|
|
{
|
2018-05-30 07:18:40 -03:00
|
|
|
uint32_t count = 0;
|
|
|
|
while (_read_rc_sitl_input()) {
|
|
|
|
count++;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (count > 100) {
|
|
|
|
::fprintf(stderr, "Read %u rc inputs\n", count);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
bool SITL_State::_read_rc_sitl_input()
|
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
struct pwm_packet {
|
2016-01-03 17:22:02 -04:00
|
|
|
uint16_t pwm[16];
|
2015-11-22 22:21:33 -04:00
|
|
|
} pwm_pkt;
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2018-05-30 07:18:40 -03:00
|
|
|
const ssize_t size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
|
2022-08-04 20:31:57 -03:00
|
|
|
|
2022-09-01 08:34:45 -03:00
|
|
|
// if we are simulating no pulses RC failure, do not update pwm_input
|
|
|
|
if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_NoPulses) {
|
|
|
|
return size != -1; // we must continue to drain _sitl_rc
|
|
|
|
}
|
|
|
|
|
2022-08-04 20:31:57 -03:00
|
|
|
if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_Throttle950) {
|
|
|
|
// discard anything we just read from the "receiver" and set
|
|
|
|
// values to bind values:
|
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(pwm_input); i++) {
|
|
|
|
pwm_input[0] = 1500; // centre all inputs
|
|
|
|
}
|
|
|
|
pwm_input[2] = 950; // reset throttle (assumed to be on channel 3...)
|
|
|
|
return size != -1; // we must continue to drain _sitl_rc
|
|
|
|
}
|
|
|
|
|
2015-11-22 22:21:33 -04:00
|
|
|
switch (size) {
|
2018-05-30 07:18:40 -03:00
|
|
|
case -1:
|
|
|
|
return false;
|
2016-01-03 17:22:02 -04:00
|
|
|
case 8*2:
|
|
|
|
case 16*2: {
|
2015-05-04 21:59:07 -03:00
|
|
|
// a packet giving the receiver PWM inputs
|
2018-07-31 07:24:18 -03:00
|
|
|
for (uint8_t i=0; i<size/2; i++) {
|
2016-01-04 09:21:54 -04:00
|
|
|
// setup the pwm input for the RC channel inputs
|
2016-05-03 23:50:02 -03:00
|
|
|
if (i < _sitl->state.rcin_chan_count) {
|
|
|
|
// we're using rc from simulator
|
|
|
|
continue;
|
|
|
|
}
|
2018-07-31 07:24:18 -03:00
|
|
|
uint16_t pwm = pwm_pkt.pwm[i];
|
|
|
|
if (pwm != 0) {
|
|
|
|
pwm_input[i] = pwm;
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
|
|
|
}
|
2018-05-30 07:18:40 -03:00
|
|
|
return true;
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
2018-05-30 07:18:40 -03:00
|
|
|
default:
|
2019-10-28 03:05:28 -03:00
|
|
|
fprintf(stderr, "Malformed SITL RC input (%ld)", (long)size);
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
2018-05-30 07:18:40 -03:00
|
|
|
return false;
|
2012-12-14 21:54:26 -04:00
|
|
|
}
|
2015-05-02 07:27:33 -03:00
|
|
|
|
2016-01-01 02:09:07 -04:00
|
|
|
/*
|
|
|
|
output current state to flightgear
|
|
|
|
*/
|
|
|
|
void SITL_State::_output_to_flightgear(void)
|
|
|
|
{
|
|
|
|
SITL::FGNetFDM fdm {};
|
|
|
|
const SITL::sitl_fdm &sfdm = _sitl->state;
|
|
|
|
|
|
|
|
fdm.version = 0x18;
|
|
|
|
fdm.padding = 0;
|
2018-10-10 16:03:11 -03:00
|
|
|
fdm.longitude = DEG_TO_RAD_DOUBLE*sfdm.longitude;
|
|
|
|
fdm.latitude = DEG_TO_RAD_DOUBLE*sfdm.latitude;
|
2016-01-01 02:09:07 -04:00
|
|
|
fdm.altitude = sfdm.altitude;
|
|
|
|
fdm.agl = sfdm.altitude;
|
|
|
|
fdm.phi = radians(sfdm.rollDeg);
|
|
|
|
fdm.theta = radians(sfdm.pitchDeg);
|
|
|
|
fdm.psi = radians(sfdm.yawDeg);
|
2022-08-25 10:30:02 -03:00
|
|
|
fdm.vcas = sfdm.velocity_air_bf.length()/0.3048;
|
2016-01-03 20:09:27 -04:00
|
|
|
if (_vehicle == ArduCopter) {
|
|
|
|
fdm.num_engines = 4;
|
|
|
|
for (uint8_t i=0; i<4; i++) {
|
|
|
|
fdm.rpm[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
fdm.num_engines = 4;
|
|
|
|
fdm.rpm[0] = constrain_float((pwm_output[2]-1000)*3, 0, 3000);
|
|
|
|
// for quadplane
|
|
|
|
fdm.rpm[1] = constrain_float((pwm_output[5]-1000)*12, 0, 12000);
|
|
|
|
fdm.rpm[2] = constrain_float((pwm_output[6]-1000)*12, 0, 12000);
|
|
|
|
fdm.rpm[3] = constrain_float((pwm_output[7]-1000)*12, 0, 12000);
|
|
|
|
}
|
2016-01-01 02:09:07 -04:00
|
|
|
fdm.ByteSwap();
|
|
|
|
|
|
|
|
fg_socket.send(&fdm, sizeof(fdm));
|
|
|
|
}
|
2015-05-02 07:27:33 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
get FDM input from a local model
|
|
|
|
*/
|
|
|
|
void SITL_State::_fdm_input_local(void)
|
|
|
|
{
|
2018-07-31 09:32:43 -03:00
|
|
|
struct sitl_input input;
|
2015-05-02 07:27:33 -03:00
|
|
|
|
2015-05-02 08:54:19 -03:00
|
|
|
// check for direct RC input
|
2021-01-18 23:36:07 -04:00
|
|
|
if (_sitl != nullptr) {
|
|
|
|
_check_rc_input();
|
|
|
|
}
|
2015-05-02 08:54:19 -03:00
|
|
|
|
|
|
|
// construct servos structure for FDM
|
2015-05-02 07:27:33 -03:00
|
|
|
_simulator_servos(input);
|
2015-05-02 08:54:19 -03:00
|
|
|
|
2021-11-01 04:03:23 -03:00
|
|
|
#if HAL_SIM_JSON_MASTER_ENABLED
|
2021-04-24 21:31:35 -03:00
|
|
|
// read servo inputs from ride along flight controllers
|
|
|
|
ride_along.receive(input);
|
2021-11-01 04:03:23 -03:00
|
|
|
#endif
|
2021-04-24 21:31:35 -03:00
|
|
|
|
2015-05-02 08:54:19 -03:00
|
|
|
// update the model
|
2019-08-15 03:52:50 -03:00
|
|
|
sitl_model->update_model(input);
|
2015-05-02 08:54:19 -03:00
|
|
|
|
|
|
|
// get FDM output from the model
|
2016-03-25 07:35:16 -03:00
|
|
|
if (_sitl) {
|
|
|
|
sitl_model->fill_fdm(_sitl->state);
|
2016-05-03 23:50:02 -03:00
|
|
|
|
2023-08-17 17:18:36 -03:00
|
|
|
#if HAL_NUM_CAN_IFACES
|
|
|
|
if (CANIface::num_interfaces() > 0) {
|
|
|
|
multicast_state_send();
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2021-07-30 07:13:59 -03:00
|
|
|
if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) {
|
2016-06-04 16:54:46 -03:00
|
|
|
for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) {
|
|
|
|
pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000;
|
|
|
|
}
|
2016-05-03 23:50:02 -03:00
|
|
|
}
|
2016-03-25 07:35:16 -03:00
|
|
|
}
|
2015-05-02 08:54:19 -03:00
|
|
|
|
2021-11-01 04:03:23 -03:00
|
|
|
#if HAL_SIM_JSON_MASTER_ENABLED
|
2021-04-24 21:31:35 -03:00
|
|
|
// output JSON state to ride along flight controllers
|
|
|
|
ride_along.send(_sitl->state,sitl_model->get_position_relhome());
|
2021-11-01 04:03:23 -03:00
|
|
|
#endif
|
2021-04-24 21:31:35 -03:00
|
|
|
|
2023-08-17 17:18:36 -03:00
|
|
|
sim_update();
|
2021-04-11 11:42:17 -03:00
|
|
|
|
2016-09-21 14:02:15 -03:00
|
|
|
if (_sitl && _use_fg_view) {
|
2016-03-25 07:35:16 -03:00
|
|
|
_output_to_flightgear();
|
|
|
|
}
|
2016-01-01 02:09:07 -04:00
|
|
|
|
2015-05-02 08:54:19 -03:00
|
|
|
// update simulation time
|
2016-03-25 07:35:16 -03:00
|
|
|
if (_sitl) {
|
|
|
|
hal.scheduler->stop_clock(_sitl->state.timestamp_us);
|
|
|
|
} else {
|
|
|
|
hal.scheduler->stop_clock(AP_HAL::micros64()+100);
|
|
|
|
}
|
2015-05-02 08:54:19 -03:00
|
|
|
|
2016-11-19 02:21:54 -04:00
|
|
|
set_height_agl();
|
|
|
|
|
2015-05-02 07:27:33 -03:00
|
|
|
_synthetic_clock_mode = true;
|
|
|
|
_update_count++;
|
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
|
|
|
|
/*
|
2015-05-02 07:27:33 -03:00
|
|
|
create sitl_input structure for sending to FDM
|
2012-12-14 21:54:26 -04:00
|
|
|
*/
|
2018-07-31 09:32:43 -03:00
|
|
|
void SITL_State::_simulator_servos(struct sitl_input &input)
|
2012-12-14 21:54:26 -04:00
|
|
|
{
|
2015-05-02 07:27:33 -03:00
|
|
|
static uint32_t last_update_usec;
|
|
|
|
|
2015-05-04 21:59:07 -03:00
|
|
|
/* this maps the registers used for PWM outputs. The RC
|
|
|
|
* driver updates these whenever it wants the channel output
|
|
|
|
* to change */
|
|
|
|
|
2017-08-07 09:05:52 -03:00
|
|
|
if (last_update_usec == 0 || !output_ready) {
|
2021-06-25 23:14:15 -03:00
|
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
|
2015-05-04 21:59:07 -03:00
|
|
|
pwm_output[i] = 1000;
|
|
|
|
}
|
|
|
|
if (_vehicle == ArduPlane) {
|
|
|
|
pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500;
|
|
|
|
}
|
2020-03-26 21:51:15 -03:00
|
|
|
if (_vehicle == Rover) {
|
2015-05-04 21:59:07 -03:00
|
|
|
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500;
|
|
|
|
}
|
2022-03-30 13:56:15 -03:00
|
|
|
if (_vehicle == ArduSub) {
|
|
|
|
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] =
|
|
|
|
pwm_output[4] = pwm_output[5] = pwm_output[6] = pwm_output[7] = 1500;
|
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2015-05-04 21:59:07 -03:00
|
|
|
// output at chosen framerate
|
2015-11-19 23:11:17 -04:00
|
|
|
uint32_t now = AP_HAL::micros();
|
2015-05-04 21:59:07 -03:00
|
|
|
last_update_usec = now;
|
2013-09-15 20:16:52 -03:00
|
|
|
|
2021-10-26 21:48:23 -03:00
|
|
|
float altitude = AP::baro().get_altitude();
|
2016-07-15 22:36:59 -03:00
|
|
|
float wind_speed = 0;
|
|
|
|
float wind_direction = 0;
|
2018-01-30 12:37:18 -04:00
|
|
|
float wind_dir_z = 0;
|
2018-04-30 12:26:15 -03:00
|
|
|
|
|
|
|
// give 5 seconds to calibrate airspeed sensor at 0 wind speed
|
|
|
|
if (wind_start_delay_micros == 0) {
|
|
|
|
wind_start_delay_micros = now;
|
|
|
|
} else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) {
|
2016-07-15 22:36:59 -03:00
|
|
|
// The EKF does not like step inputs so this LPF keeps it happy.
|
2018-01-30 12:37:18 -04:00
|
|
|
wind_speed = _sitl->wind_speed_active = (0.95f*_sitl->wind_speed_active) + (0.05f*_sitl->wind_speed);
|
2016-07-15 22:36:59 -03:00
|
|
|
wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction);
|
2018-01-30 12:37:18 -04:00
|
|
|
wind_dir_z = _sitl->wind_dir_z_active = (0.95f*_sitl->wind_dir_z_active) + (0.05f*_sitl->wind_dir_z);
|
2018-04-30 12:26:15 -03:00
|
|
|
|
|
|
|
// pass wind into simulators using different wind types via param SIM_WIND_T*.
|
|
|
|
switch (_sitl->wind_type) {
|
2021-07-30 07:13:59 -03:00
|
|
|
case SITL::SIM::WIND_TYPE_SQRT:
|
2018-04-30 12:26:15 -03:00
|
|
|
if (altitude < _sitl->wind_type_alt) {
|
|
|
|
wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0));
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2021-07-30 07:13:59 -03:00
|
|
|
case SITL::SIM::WIND_TYPE_COEF:
|
2018-04-30 12:26:15 -03:00
|
|
|
wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef;
|
|
|
|
break;
|
2016-07-15 22:36:59 -03:00
|
|
|
|
2021-07-30 07:13:59 -03:00
|
|
|
case SITL::SIM::WIND_TYPE_NO_LIMIT:
|
2018-04-30 12:26:15 -03:00
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// never allow negative wind velocity
|
|
|
|
wind_speed = MAX(wind_speed, 0);
|
|
|
|
}
|
2016-07-15 22:36:59 -03:00
|
|
|
|
2015-05-22 22:04:13 -03:00
|
|
|
input.wind.speed = wind_speed;
|
2016-07-15 22:36:59 -03:00
|
|
|
input.wind.direction = wind_direction;
|
2016-03-25 07:35:16 -03:00
|
|
|
input.wind.turbulence = _sitl?_sitl->wind_turbulance:0;
|
2018-01-30 12:37:18 -04:00
|
|
|
input.wind.dir_z = wind_dir_z;
|
2015-05-22 22:04:13 -03:00
|
|
|
|
2021-06-25 23:14:15 -03:00
|
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
|
2015-05-04 21:59:07 -03:00
|
|
|
if (pwm_output[i] == 0xFFFF) {
|
|
|
|
input.servos[i] = 0;
|
|
|
|
} else {
|
|
|
|
input.servos[i] = pwm_output[i];
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-06-28 19:24:15 -03:00
|
|
|
if (_sitl != nullptr) {
|
|
|
|
// FETtec ESC simulation support. Input signals of 1000-2000
|
|
|
|
// are positive thrust, 0 to 1000 are negative thrust. Deeper
|
|
|
|
// changes required to support negative thrust - potentially
|
|
|
|
// adding a field to input.
|
|
|
|
if (_sitl != nullptr) {
|
|
|
|
if (_sitl->fetteconewireesc_sim.enabled()) {
|
|
|
|
_sitl->fetteconewireesc_sim.update_sitl_input_pwm(input);
|
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
|
|
|
|
if (input.servos[i] != 0 && input.servos[i] < 1000) {
|
|
|
|
AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-03-26 00:17:12 -03:00
|
|
|
float engine_mul = _sitl?_sitl->engine_mul.get():1;
|
2017-03-20 17:41:09 -03:00
|
|
|
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
|
2019-12-24 17:19:05 -04:00
|
|
|
float throttle = 0.0f;
|
2016-03-25 07:35:16 -03:00
|
|
|
|
2017-04-07 22:50:28 -03:00
|
|
|
if (engine_fail >= ARRAY_SIZE(input.servos)) {
|
2017-03-20 17:41:09 -03:00
|
|
|
engine_fail = 0;
|
|
|
|
}
|
2017-04-07 22:50:28 -03:00
|
|
|
// apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
|
2020-03-26 21:51:15 -03:00
|
|
|
if (_vehicle != Rover) {
|
2017-03-05 09:20:20 -04:00
|
|
|
input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000;
|
|
|
|
} else {
|
|
|
|
input.servos[engine_fail] = static_cast<uint16_t>(((input.servos[engine_fail] - 1500) * engine_mul) + 1500);
|
|
|
|
}
|
2019-12-24 17:19:05 -04:00
|
|
|
|
2015-05-04 21:59:07 -03:00
|
|
|
if (_vehicle == ArduPlane) {
|
2020-01-16 15:00:58 -04:00
|
|
|
float forward_throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
|
|
|
|
// do a little quadplane dance
|
|
|
|
float hover_throttle = 0.0f;
|
|
|
|
uint8_t running_motors = 0;
|
2022-08-18 04:22:01 -03:00
|
|
|
uint32_t mask = _sitl->state.motor_mask;
|
|
|
|
uint8_t bit;
|
|
|
|
while ((bit = __builtin_ffs(mask)) != 0) {
|
|
|
|
uint8_t motor = bit-1;
|
|
|
|
mask &= ~(1U<<motor);
|
|
|
|
float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
|
2020-01-16 15:00:58 -04:00
|
|
|
// update motor_on flag
|
|
|
|
if (!is_zero(motor_throttle)) {
|
|
|
|
hover_throttle += motor_throttle;
|
|
|
|
running_motors++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (running_motors > 0) {
|
|
|
|
hover_throttle /= running_motors;
|
|
|
|
}
|
|
|
|
if (!is_zero(forward_throttle)) {
|
|
|
|
throttle = forward_throttle;
|
|
|
|
} else {
|
|
|
|
throttle = hover_throttle;
|
|
|
|
}
|
2020-03-26 21:51:15 -03:00
|
|
|
} else if (_vehicle == Rover) {
|
2017-03-05 09:20:20 -04:00
|
|
|
input.servos[2] = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000));
|
|
|
|
input.servos[0] = static_cast<uint16_t>(constrain_int16(input.servos[0], 1000, 2000));
|
2019-12-24 17:19:05 -04:00
|
|
|
throttle = fabsf((input.servos[2] - 1500) / 500.0f);
|
2015-05-04 21:59:07 -03:00
|
|
|
} else {
|
2013-05-19 02:29:08 -03:00
|
|
|
// run checks on each motor
|
2019-12-24 17:19:05 -04:00
|
|
|
uint8_t running_motors = 0;
|
2022-08-18 04:22:01 -03:00
|
|
|
uint32_t mask = _sitl->state.motor_mask;
|
|
|
|
uint8_t bit;
|
|
|
|
while ((bit = __builtin_ffs(mask)) != 0) {
|
|
|
|
const uint8_t motor = bit-1;
|
|
|
|
mask &= ~(1U<<motor);
|
|
|
|
float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
|
2013-05-19 02:29:08 -03:00
|
|
|
// update motor_on flag
|
2019-12-24 17:19:05 -04:00
|
|
|
if (!is_zero(motor_throttle)) {
|
|
|
|
throttle += motor_throttle;
|
|
|
|
running_motors++;
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
|
|
|
}
|
2019-12-24 17:19:05 -04:00
|
|
|
if (running_motors > 0) {
|
|
|
|
throttle /= running_motors;
|
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
2016-03-25 07:35:16 -03:00
|
|
|
if (_sitl) {
|
2019-12-24 17:19:05 -04:00
|
|
|
_sitl->throttle = throttle;
|
2016-03-25 07:35:16 -03:00
|
|
|
}
|
2012-12-14 21:54:26 -04:00
|
|
|
|
2023-08-17 18:55:20 -03:00
|
|
|
update_voltage_current(input, throttle);
|
2015-05-02 07:27:33 -03:00
|
|
|
}
|
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
void SITL_State::init(int argc, char * const argv[])
|
|
|
|
{
|
2019-09-15 03:46:13 -03:00
|
|
|
pwm_input[0] = pwm_input[1] = pwm_input[3] = 1500;
|
|
|
|
pwm_input[4] = pwm_input[7] = 1800;
|
|
|
|
pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
|
2012-12-17 23:56:21 -04:00
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
_scheduler = Scheduler::from(hal.scheduler);
|
2015-05-04 21:59:07 -03:00
|
|
|
_parse_command_line(argc, argv);
|
2012-12-14 21:54:26 -04:00
|
|
|
}
|
|
|
|
|
2015-01-03 06:47:54 -04:00
|
|
|
/*
|
2016-11-19 02:21:54 -04:00
|
|
|
set height above the ground in meters
|
2015-01-03 06:47:54 -04:00
|
|
|
*/
|
2016-11-19 02:21:54 -04:00
|
|
|
void SITL_State::set_height_agl(void)
|
2015-01-03 06:47:54 -04:00
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
static float home_alt = -1;
|
2015-01-03 06:47:54 -04:00
|
|
|
|
2018-11-04 01:31:04 -04:00
|
|
|
if (!_sitl) {
|
|
|
|
// in example program
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2018-08-14 23:35:38 -03:00
|
|
|
if (is_equal(home_alt, -1.0f) && _sitl->state.altitude > 0) {
|
2015-01-03 06:47:54 -04:00
|
|
|
// remember home altitude as first non-zero altitude
|
2015-05-04 21:59:07 -03:00
|
|
|
home_alt = _sitl->state.altitude;
|
2015-01-03 06:47:54 -04:00
|
|
|
}
|
|
|
|
|
2016-06-03 15:07:36 -03:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
2020-07-20 22:56:10 -03:00
|
|
|
if (_sitl != nullptr &&
|
2018-03-31 02:11:31 -03:00
|
|
|
_sitl->terrain_enable) {
|
2015-01-03 06:47:54 -04:00
|
|
|
// get height above terrain from AP_Terrain. This assumes
|
|
|
|
// AP_Terrain is working
|
|
|
|
float terrain_height_amsl;
|
2023-02-02 18:58:38 -04:00
|
|
|
Location location;
|
2015-01-03 06:47:54 -04:00
|
|
|
location.lat = _sitl->state.latitude*1.0e7;
|
|
|
|
location.lng = _sitl->state.longitude*1.0e7;
|
|
|
|
|
2020-07-20 22:56:10 -03:00
|
|
|
AP_Terrain *_terrain = AP_Terrain::get_singleton();
|
|
|
|
if (_terrain != nullptr &&
|
2022-03-27 18:27:33 -03:00
|
|
|
_terrain->height_amsl(location, terrain_height_amsl, false)) {
|
2016-11-19 02:21:54 -04:00
|
|
|
_sitl->height_agl = _sitl->state.altitude - terrain_height_amsl;
|
|
|
|
return;
|
2015-01-03 06:47:54 -04:00
|
|
|
}
|
|
|
|
}
|
2016-06-03 15:07:36 -03:00
|
|
|
#endif
|
2015-01-03 06:47:54 -04:00
|
|
|
|
2018-03-31 02:11:31 -03:00
|
|
|
if (_sitl != nullptr) {
|
|
|
|
// fall back to flat earth model
|
|
|
|
_sitl->height_agl = _sitl->state.altitude - home_alt;
|
|
|
|
}
|
2015-01-03 06:47:54 -04:00
|
|
|
}
|
|
|
|
|
2023-08-17 17:18:36 -03:00
|
|
|
/*
|
|
|
|
open multicast UDP
|
|
|
|
*/
|
|
|
|
void SITL_State::multicast_state_open(void)
|
|
|
|
{
|
|
|
|
struct sockaddr_in sockaddr {};
|
|
|
|
int ret;
|
|
|
|
|
|
|
|
#ifdef HAVE_SOCK_SIN_LEN
|
|
|
|
sockaddr.sin_len = sizeof(sockaddr);
|
|
|
|
#endif
|
|
|
|
sockaddr.sin_port = htons(SITL_MCAST_PORT);
|
|
|
|
sockaddr.sin_family = AF_INET;
|
|
|
|
sockaddr.sin_addr.s_addr = inet_addr(SITL_MCAST_IP);
|
|
|
|
|
|
|
|
mc_out_fd = socket(AF_INET, SOCK_DGRAM, 0);
|
|
|
|
if (mc_out_fd == -1) {
|
|
|
|
fprintf(stderr, "socket failed - %s\n", strerror(errno));
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
ret = fcntl(mc_out_fd, F_SETFD, FD_CLOEXEC);
|
|
|
|
if (ret == -1) {
|
|
|
|
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
// try to setup for broadcast, this may fail if insufficient privileges
|
|
|
|
int one = 1;
|
|
|
|
setsockopt(mc_out_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
|
|
|
|
|
|
|
|
ret = connect(mc_out_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
|
|
|
|
if (ret == -1) {
|
|
|
|
fprintf(stderr, "udp connect failed on port %u - %s\n",
|
|
|
|
(unsigned)ntohs(sockaddr.sin_port),
|
|
|
|
strerror(errno));
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
send out SITL state as multicast UDP
|
|
|
|
*/
|
|
|
|
void SITL_State::multicast_state_send(void)
|
|
|
|
{
|
|
|
|
if (_sitl == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (mc_out_fd == -1) {
|
|
|
|
multicast_state_open();
|
|
|
|
}
|
|
|
|
const auto &sfdm = _sitl->state;
|
|
|
|
send(mc_out_fd, (void*)&sfdm, sizeof(sfdm), 0);
|
|
|
|
}
|
|
|
|
|
2012-12-14 21:54:26 -04:00
|
|
|
#endif
|