Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: add and use AP_RCProtocol_UDP

This commit is contained in:
Peter Barker 2024-03-14 16:47:23 +11:00 committed by Peter Barker
parent 4a996c81fb
commit 4005bce85c
3 changed files with 2 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -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
@ -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) {
@ -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 */
@ -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) {
@ -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
*/
@ -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);
@ -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());
@ -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);
}

5
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -59,7 +59,6 @@ private:
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);
@ -68,8 +67,6 @@ private:
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);
@ -85,8 +82,6 @@ private:
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;

8
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -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;
}