Browse Source

AP_HAL_SITL: add slave JSON instances

gps-1.3.1
Iampete1 4 years ago committed by Andrew Tridgell
parent
commit
5ff0f42372
  1. 6
      libraries/AP_HAL_SITL/SITL_State.cpp
  2. 5
      libraries/AP_HAL_SITL/SITL_State.h
  3. 10
      libraries/AP_HAL_SITL/SITL_cmdline.cpp

6
libraries/AP_HAL_SITL/SITL_State.cpp

@ -613,6 +613,9 @@ void SITL_State::_fdm_input_local(void) @@ -613,6 +613,9 @@ void SITL_State::_fdm_input_local(void)
// construct servos structure for FDM
_simulator_servos(input);
// read servo inputs from ride along flight controllers
ride_along.receive(input);
// update the model
sitl_model->update_model(input);
@ -628,6 +631,9 @@ void SITL_State::_fdm_input_local(void) @@ -628,6 +631,9 @@ void SITL_State::_fdm_input_local(void)
}
}
// output JSON state to ride along flight controllers
ride_along.send(_sitl->state,sitl_model->get_position_relhome());
if (gimbal != nullptr) {
gimbal->update();
}

5
libraries/AP_HAL_SITL/SITL_State.h

@ -308,7 +308,10 @@ private: @@ -308,7 +308,10 @@ private:
// simulated VectorNav system:
SITL::VectorNav *vectornav;
// Ride along instances via JSON SITL backend
SITL::JSON_Master ride_along;
// output socket for flightgear viewing
SocketAPM fg_socket{true};

10
libraries/AP_HAL_SITL/SITL_cmdline.cpp

@ -113,6 +113,7 @@ void SITL_State::_usage(void) @@ -113,6 +113,7 @@ void SITL_State::_usage(void)
"\t--irlock-port PORT set port num for irlock\n"
"\t--start-time TIMESTR set simulation start time in UNIX timestamp\n"
"\t--sysid ID set SYSID_THISMAV\n"
"\t--slave number set the number of JSON slaves\n"
);
}
@ -260,6 +261,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) @@ -260,6 +261,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
CMDLINE_IRLOCK_PORT,
CMDLINE_START_TIME,
CMDLINE_SYSID,
CMDLINE_SLAVE,
};
const struct GetOptLong::option options[] = {
@ -307,6 +309,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) @@ -307,6 +309,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{"irlock-port", true, 0, CMDLINE_IRLOCK_PORT},
{"start-time", true, 0, CMDLINE_START_TIME},
{"sysid", true, 0, CMDLINE_SYSID},
{"slave", true, 0, CMDLINE_SLAVE},
{0, false, 0, 0}
};
@ -462,6 +465,13 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) @@ -462,6 +465,13 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
case 'h':
_usage();
exit(0);
case CMDLINE_SLAVE: {
const int32_t slaves = atoi(gopt.optarg);
if (slaves > 0) {
ride_along.init(slaves);
}
break;
}
default:
_usage();
exit(1);

Loading…
Cancel
Save