diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 1d1f86167a..1bdd9b75b6 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -249,10 +249,6 @@ private: _perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")), _perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")) { - for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { - _actuator_outputs_sub[i] = -1; - } - simulator::RawGPSData gps_data{}; gps_data.eph = UINT16_MAX; gps_data.epv = UINT16_MAX; @@ -332,7 +328,7 @@ private: orb_advert_t _lpos_pub{nullptr}; // uORB subscription handlers - int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES] {-1, -1, -1, -1}; + int _actuator_outputs_sub{-1}; int _vehicle_attitude_sub{-1}; int _manual_sub{-1}; int _vehicle_status_sub{-1}; @@ -347,7 +343,6 @@ private: // uORB data containers input_rc_s _rc_input {}; - actuator_outputs_s _actuators[ORB_MULTI_MAX_INSTANCES] {}; vehicle_attitude_s _attitude {}; manual_control_setpoint_s _manual {}; vehicle_status_s _vehicle_status {}; @@ -365,7 +360,7 @@ private: void request_hil_state_quaternion(); void pollForMAVLinkMessages(bool publish); - void pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index); + mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators); void send_mavlink_message(const mavlink_message_t &aMsg); void update_sensors(mavlink_hil_sensor_t *imu); void update_gps(mavlink_hil_gps_t *gps_sim); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 1c1b4bb92c..ed87153b71 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -73,9 +73,12 @@ const unsigned mode_flag_armed = 128; const unsigned mode_flag_custom = 1; using namespace simulator; +using namespace time_literals; -void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsigned index) +mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const actuator_outputs_s &actuators) { + mavlink_hil_actuator_controls_t msg{}; + msg.time_usec = hrt_absolute_time() + hrt_absolute_time_offset(); bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); @@ -123,14 +126,14 @@ void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsi } for (unsigned i = 0; i < 16; i++) { - if (_actuators[index].output[i] > PWM_DEFAULT_MIN / 2) { + if (actuators.output[i] > PWM_DEFAULT_MIN / 2) { if (i < n) { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ - msg.controls[i] = (_actuators[index].output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + msg.controls[i] = (actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } else { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ - msg.controls[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + msg.controls[i] = (actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } } else { @@ -143,14 +146,14 @@ void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsi /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 16; i++) { - if (_actuators[index].output[i] > PWM_DEFAULT_MIN / 2) { + if (actuators.output[i] > PWM_DEFAULT_MIN / 2) { if (i != 4) { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for normal channels */ - msg.controls[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + msg.controls[i] = (actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } else { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for throttle */ - msg.controls[i] = (_actuators[index].output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + msg.controls[i] = (actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } } else { @@ -163,24 +166,30 @@ void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsi msg.mode = mode_flag_custom; msg.mode |= (armed) ? mode_flag_armed : 0; msg.flags = 0; + + return msg; } void Simulator::send_controls() { - for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { + // copy new actuator data if available + bool updated = false; + orb_check(_actuator_outputs_sub, &updated); - if (_actuator_outputs_sub[i] < 0 || _actuators[i].timestamp == 0) { - continue; + if (updated) { + actuator_outputs_s actuators{}; + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &actuators); + + if (actuators.timestamp > 0) { + const mavlink_hil_actuator_controls_t hil_act_control = actuator_controls_from_outputs(actuators); + + mavlink_message_t message{}; + mavlink_msg_hil_actuator_controls_encode(0, 200, &message, &hil_act_control); + + PX4_DEBUG("sending controls t=%ld (%ld)", actuators.timestamp, hil_act_control.time_usec); + + send_mavlink_message(message); } - - mavlink_hil_actuator_controls_t hil_act_control = {}; - hil_act_control.time_usec = _actuators[i].timestamp + hrt_absolute_time_offset(); - - mavlink_message_t message = {}; - pack_actuator_message(hil_act_control, i); - mavlink_msg_hil_actuator_controls_encode(0, 200, &message, &hil_act_control); - - send_mavlink_message(message); } } @@ -303,7 +312,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) float step = diff / 4000.0f; if (step > 1.1f || step < 0.9f) { - PX4_INFO("time_usec: %lu, diff: %lu, step: %.2f", now_us, diff, step); + PX4_INFO("HIL_SENSOR: imu time_usec: %lu, time_usec: %lu, diff: %lu, step: %.2f", imu.time_usec, now_us, diff, step); } last_time = now_us; @@ -316,7 +325,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) update_sensors(&imu); // battery simulation (limit update to 100Hz) - if (hrt_elapsed_time(&_battery_status.timestamp) >= 10000) { + if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { const float discharge_interval_us = _battery_drain_interval_s.get() * 1000 * 1000; @@ -528,16 +537,7 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) void Simulator::poll_topics() { // copy new actuator data if available - bool updated; - - for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { - - orb_check(_actuator_outputs_sub[i], &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub[i], &_actuators[i]); - } - } + bool updated = false; orb_check(_vehicle_status_sub, &updated); @@ -566,12 +566,8 @@ void Simulator::send() // Without this, we get stuck at px4_poll which waits for a time update. send_heartbeat(); - // We then send the controls once which will notify the SITL simulator - // that our time is 0. - send_controls(); - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _actuator_outputs_sub[0]; + fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; while (true) { @@ -735,7 +731,9 @@ void Simulator::pollForMAVLinkMessages(bool publish) struct sched_param param; (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_DEFAULT; + // sender thread should run immediately after new outputs are available + // to send the lockstep update to the simulation + param.sched_priority = SCHED_PRIORITY_ACTUATOR_OUTPUTS + 1; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); struct pollfd fds[2] = {}; @@ -763,10 +761,8 @@ void Simulator::pollForMAVLinkMessages(bool publish) #endif // subscribe to topics - for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { - _actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i); - } - + // only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // got data from simulator, now activate the sending thread @@ -791,14 +787,13 @@ void Simulator::pollForMAVLinkMessages(bool publish) } if (pret < 0) { - PX4_WARN("simulator mavlink: poll error %d, %d", pret, errno); + PX4_ERR("poll error %d, %d", pret, errno); continue; } if (fds[0].revents & POLLIN) { - int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0, - (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen); + int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen); if (len > 0) { mavlink_message_t msg;