Skip to content

Commit

Permalink
AP_HAL_SITL: add and use AP_RCProtocol_UDP_Packed16BitValues
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Mar 14, 2024
1 parent 7796bc5 commit c1585f7
Show file tree
Hide file tree
Showing 3 changed files with 2 additions and 126 deletions.
115 changes: 2 additions & 113 deletions libraries/AP_HAL_SITL/SITL_State.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,6 @@ void SITL_State::_sitl_setup()
_parent_pid = getppid();
#endif

_setup_fdm();
fprintf(stdout, "Starting SITL input\n");

// find the barometer object if it exists
Expand Down Expand Up @@ -101,6 +100,8 @@ void SITL_State::_sitl_setup()

fprintf(stdout, "Using Irlock at port : %d\n", _irlock_port);
_sitl->irlock_port = _irlock_port;

_sitl->rcin_port = _rcin_port;
}

if (_synthetic_clock_mode) {
Expand All @@ -110,38 +111,11 @@ void SITL_State::_sitl_setup()
}


/*
setup a SITL FDM listening UDP port
*/
bool SITL_State::_setup_fdm(void)
{
if (_rc_in_started) {
return true;
}
if (!_sitl_rc_in.reuseaddress()) {
return false;
}
if (!_sitl_rc_in.bind("0.0.0.0", _rcin_port)) {
return false;
}
if (!_sitl_rc_in.set_blocking(false)) {
return false;
}
if (!_sitl_rc_in.set_cloexec()) {
return false;
}
_rc_in_started = true;
return true;
}


/*
step the FDM by one time step
*/
void SITL_State::_fdm_input_step(void)
{
static uint32_t last_pwm_input = 0;

_fdm_input_local();

/* make sure we die if our parent dies */
Expand All @@ -153,12 +127,6 @@ void SITL_State::_fdm_input_step(void)
return;
}

// simulate RC input at 50Hz
if (AP_HAL::millis() - last_pwm_input >= 20 && _sitl != nullptr && _sitl->rc_fail != SITL::SIM::SITL_RCFail_NoPulses) {
last_pwm_input = AP_HAL::millis();
new_rc_input = true;
}

_scheduler->sitl_begin_atomic();

if (_update_count == 0 && _sitl != nullptr) {
Expand Down Expand Up @@ -225,72 +193,6 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
}
}

/*
check for a SITL RC input packet
*/
void SITL_State::_check_rc_input(void)
{
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()
{
struct pwm_packet {
uint16_t pwm[16];
} pwm_pkt;

if (!_setup_fdm()) {
return false;
}
const ssize_t size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);

// 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
}

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
}

switch (size) {
case -1:
return false;
case 8*2:
case 16*2: {
// a packet giving the receiver PWM inputs
for (uint8_t i=0; i<size/2; i++) {
// setup the pwm input for the RC channel inputs
if (i < _sitl->state.rcin_chan_count) {
// we're using rc from simulator
continue;
}
uint16_t pwm = pwm_pkt.pwm[i];
if (pwm != 0) {
pwm_input[i] = pwm;
}
}
return true;
}
default:
fprintf(stderr, "Malformed SITL RC input (%ld)", (long)size);
}
return false;
}

/*
output current state to flightgear
*/
Expand Down Expand Up @@ -337,9 +239,6 @@ void SITL_State::_fdm_input_local(void)
}
struct sitl_input input;

// check for direct RC input
_check_rc_input();

// construct servos structure for FDM
_simulator_servos(input);

Expand All @@ -364,12 +263,6 @@ void SITL_State::_fdm_input_local(void)
}
#endif

if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) {
for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) {
pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000;
}
}

#if HAL_SIM_JSON_MASTER_ENABLED
// output JSON state to ride along flight controllers
ride_along.send(_sitl->state,sitl_model->get_position_relhome());
Expand Down Expand Up @@ -560,10 +453,6 @@ void SITL_State::_simulator_servos(struct sitl_input &input)

void SITL_State::init(int argc, char * const argv[])
{
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;

_scheduler = Scheduler::from(hal.scheduler);
_parse_command_line(argc, argv);
}
Expand Down
5 changes: 0 additions & 5 deletions libraries/AP_HAL_SITL/SITL_State.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,6 @@ class HALSITL::SITL_State : public SITL_State_Common {
void _set_param_default(const char *parm);
void _usage(void);
void _sitl_setup();
bool _setup_fdm(void);
void _setup_timer(void);
void _setup_adc(void);

Expand All @@ -68,8 +67,6 @@ class HALSITL::SITL_State : public SITL_State_Common {
void _set_signal_handlers(void) const;

void _update_airspeed(float airspeed);
void _check_rc_input(void);
bool _read_rc_sitl_input();
void _fdm_input_local(void);
void _output_to_flightgear(void);
void _simulator_servos(struct sitl_input &input);
Expand All @@ -85,8 +82,6 @@ class HALSITL::SITL_State : public SITL_State_Common {

Scheduler *_scheduler;

SocketAPM_native _sitl_rc_in{true};
bool _rc_in_started;
uint16_t _rcin_port;
uint16_t _fg_view_port;
uint16_t _irlock_port;
Expand Down
8 changes: 0 additions & 8 deletions libraries/AP_HAL_SITL/SITL_cmdline.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -622,18 +622,10 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_vehicle = ArduCopter;
} else if (strcmp(vehicle_str, "Rover") == 0) {
_vehicle = Rover;
// set right default throttle for rover (allowing for reverse)
pwm_input[2] = 1500;
} else if (strcmp(vehicle_str, "ArduSub") == 0) {
_vehicle = ArduSub;
for(uint8_t i = 0; i < 8; i++) {
pwm_input[i] = 1500;
}
} else if (strcmp(vehicle_str, "Blimp") == 0) {
_vehicle = Blimp;
for(uint8_t i = 0; i < 8; i++) {
pwm_input[i] = 1500;
}
} else {
_vehicle = ArduPlane;
}
Expand Down

0 comments on commit c1585f7

Please sign in to comment.