fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Simulator: Send all actuator output groups

Browse Source
This commit is contained in:
Lorenz Meier 2016-06-18 01:50:42 +02:00
parent c285e231b7
commit 85e3073f14
2 changed files with 33 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/simulator/simulator.h
View File

@ -243,7 +243,7 @@ private:
#ifndef __PX4_QURT
,
_rc_channels_pub(nullptr),
_actuator_outputs_sub(-1),
_actuator_outputs_sub{},
_vehicle_attitude_sub(-1),
_manual_sub(-1),
_vehicle_status_sub(-1),
@ -253,7 +253,11 @@ private:
_manual{},
_vehicle_status{}
#endif
{}
{
for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
_actuator_outputs_sub[i] = -1;
}
}
~Simulator() { _instance = NULL; }
void initializeSensorData();
@ -299,14 +303,14 @@ private:
orb_advert_t _rc_channels_pub;
// uORB subscription handlers
int _actuator_outputs_sub;
int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES];
int _vehicle_attitude_sub;
int _manual_sub;
int _vehicle_status_sub;
// uORB data containers
struct rc_input_values _rc_input;
struct actuator_outputs_s _actuators;
struct actuator_outputs_s _actuators[ORB_MULTI_MAX_INSTANCES];
struct vehicle_attitude_s _attitude;
struct manual_control_setpoint_s _manual;
struct vehicle_status_s _vehicle_status;
@ -316,7 +320,7 @@ private:
void send_controls();
void pollForMAVLinkMessages(bool publish, int udp_port);
void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
void pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index);
void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
void update_sensors(mavlink_hil_sensor_t *imu);
void update_gps(mavlink_hil_gps_t *gps_sim);

35
src/modules/simulator/simulator_mavlink.cpp
View File

@ -77,7 +77,7 @@ const unsigned mode_flag_custom = 1;
using namespace simulator;
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index)
{
// reset state
memset(&actuator_msg, 0, sizeof(actuator_msg));
@ -90,7 +90,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
// scale PWM out 900..2100 us to -1..1 */
out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
out[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
if (!PX4_ISFINITE(out[i])) {
out[i] = -1.0f;
@ -107,14 +107,21 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
actuator_msg.aux4 = out[7];
actuator_msg.mode = mode_flag_custom;
actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
actuator_msg.nav_mode = 0;
actuator_msg.nav_mode = index; // XXX this indicates the output group in our use of the message
}
void Simulator::send_controls()
{
mavlink_hil_controls_t msg;
pack_actuator_message(msg);
send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
if (_actuator_outputs_sub[i] < 0 || _actuators[i].timestamp == 0) {
continue;
}
mavlink_hil_controls_t msg;
pack_actuator_message(msg, i);
send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
}
}
static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels)
@ -341,10 +348,14 @@ void Simulator::poll_topics()
{
// copy new actuator data if available
bool updated;
orb_check(_actuator_outputs_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
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]);
}
}
orb_check(_vehicle_status_sub, &updated);
@ -363,7 +374,7 @@ void *Simulator::sending_trampoline(void *)
void Simulator::send()
{
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _actuator_outputs_sub;
fds[0].fd = _actuator_outputs_sub[0];
fds[0].events = POLLIN;
@ -554,7 +565,9 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
(void)hrt_reset();
// subscribe to topics
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
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);
}
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
// got data from simulator, now activate the sending thread