From 010c9e937b23988b222de6a1f714b57690f7742f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 1 Sep 2016 16:34:46 +0200
Subject: [PATCH] SITL: switch to HIL_ACTUATOR_CONTROLS mavlink message & add
pwm_out_sim support for 16 outputs
---
posix-configs/SITL/init/ekf2/typhoon_h480 | 2 +-
posix-configs/SITL/init/lpe/typhoon_h480 | 2 +-
src/drivers/pwm_out_sim/pwm_out_sim.cpp | 13 +-
src/modules/mavlink/mavlink_main.cpp | 4 +-
src/modules/mavlink/mavlink_messages.cpp | 154 ++++++++++++++++++++
src/modules/simulator/simulator.h | 2 +-
src/modules/simulator/simulator_mavlink.cpp | 27 +---
7 files changed, 179 insertions(+), 25 deletions(-)
diff --git a/posix-configs/SITL/init/ekf2/typhoon_h480 b/posix-configs/SITL/init/ekf2/typhoon_h480
index 7d1a9c9e65..b673e8d5c1 100644
--- a/posix-configs/SITL/init/ekf2/typhoon_h480
+++ b/posix-configs/SITL/init/ekf2/typhoon_h480
@@ -50,7 +50,7 @@ accelsim start
barosim start
adcsim start
gpssim start
-pwm_out_sim mode_pwm
+pwm_out_sim mode_pwm16
sleep 1
sensors start
commander start
diff --git a/posix-configs/SITL/init/lpe/typhoon_h480 b/posix-configs/SITL/init/lpe/typhoon_h480
index a015b7e639..a0195ece16 100644
--- a/posix-configs/SITL/init/lpe/typhoon_h480
+++ b/posix-configs/SITL/init/lpe/typhoon_h480
@@ -50,7 +50,7 @@ accelsim start
barosim start
adcsim start
gpssim start
-pwm_out_sim mode_pwm
+pwm_out_sim mode_pwm16
sleep 1
sensors start
commander start
diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index af264d966c..1df5f4bc8b 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -471,6 +471,10 @@ PWMSim::task_main()
num_outputs = 8;
break;
+ case MODE_16PWM:
+ num_outputs = 16;
+ break;
+
default:
num_outputs = 0;
break;
@@ -729,7 +733,11 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
- if (_mode == MODE_8PWM) {
+ if (_mode == MODE_16PWM) {
+ *(unsigned *)arg = 16;
+
+ } else if (_mode == MODE_8PWM) {
+
*(unsigned *)arg = 8;
} else if (_mode == MODE_4PWM) {
@@ -1000,6 +1008,9 @@ pwm_out_sim_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_port2_pwm16")) {
new_mode = PORT2_8PWM;
+
+ } else if (!strcmp(verb, "mode_pwm16")) {
+ new_mode = PORT2_16PWM;
}
/* was a new mode set? */
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a27361b551..a5a7a3c969 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -851,13 +851,13 @@ Mavlink::set_hil_enabled(bool hil_enabled)
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
- configure_stream("HIL_CONTROLS", 200.0f);
+ configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f);
}
/* disable HIL */
if (!hil_enabled && _hil_enabled) {
_hil_enabled = false;
- configure_stream("HIL_CONTROLS", 0.0f);
+ configure_stream("HIL_ACTUATOR_CONTROLS", 0.0f);
} else {
ret = ERROR;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 56a60ba3e4..d767ed04c7 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2053,6 +2053,7 @@ protected:
};
+//TODO: this is deprecated (09.2016). Remove it some time in the future...
class MavlinkStreamHILControls : public MavlinkStream
{
public:
@@ -2216,6 +2217,158 @@ protected:
}
};
+class MavlinkStreamHILActuatorControls : public MavlinkStream
+{
+public:
+ const char *get_name() const
+ {
+ return MavlinkStreamHILActuatorControls::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "HIL_ACTUATOR_CONTROLS";
+ }
+
+ static uint8_t get_id_static()
+ {
+ return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS;
+ }
+
+ uint8_t get_id()
+ {
+ return get_id_static();
+ }
+
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamHILActuatorControls(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+ }
+
+private:
+ MavlinkOrbSubscription *_status_sub;
+ uint64_t _status_time;
+
+ MavlinkOrbSubscription *_act_sub;
+ uint64_t _act_time;
+
+ /* do not allow top copying this class */
+ MavlinkStreamHILActuatorControls(MavlinkStreamHILActuatorControls &);
+ MavlinkStreamHILActuatorControls &operator = (const MavlinkStreamHILActuatorControls &);
+
+protected:
+ explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+ _status_time(0),
+ _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
+ _act_time(0)
+ {}
+
+ void send(const hrt_abstime t)
+ {
+ struct vehicle_status_s status;
+ struct actuator_outputs_s act;
+
+ bool updated = _act_sub->update(&_act_time, &act);
+ updated |= _status_sub->update(&_status_time, &status);
+
+ if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+ /* translate the current syste state to mavlink state and mode */
+ uint8_t mavlink_state;
+ uint8_t mavlink_base_mode;
+ uint32_t mavlink_custom_mode;
+ mavlink_hil_actuator_controls_t msg;
+
+ get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+ const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
+
+ unsigned system_type = _mavlink->get_system_type();
+
+ /* scale outputs depending on system type */
+ if (system_type == MAV_TYPE_QUADROTOR ||
+ system_type == MAV_TYPE_HEXAROTOR ||
+ system_type == MAV_TYPE_OCTOROTOR ||
+ system_type == MAV_TYPE_VTOL_DUOROTOR ||
+ system_type == MAV_TYPE_VTOL_QUADROTOR) {
+
+ /* multirotors: set number of rotor outputs depending on type */
+
+ unsigned n;
+
+ switch (system_type) {
+ case MAV_TYPE_QUADROTOR:
+ n = 4;
+ break;
+
+ case MAV_TYPE_HEXAROTOR:
+ n = 6;
+ break;
+
+ case MAV_TYPE_VTOL_DUOROTOR:
+ n = 2;
+ break;
+
+ case MAV_TYPE_VTOL_QUADROTOR:
+ n = 4;
+ break;
+
+ default:
+ n = 8;
+ break;
+ }
+
+ for (unsigned i = 0; i < 16; i++) {
+ if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+ if (i < n) {
+ /* scale PWM out 900..2100 us to 0..1 for rotors */
+ msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+
+ } else {
+ /* scale PWM out 900..2100 us to -1..1 for other channels */
+ msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+ }
+
+ } else {
+ /* send 0 when disarmed and for disabled channels */
+ msg.controls[i] = 0.0f;
+ }
+ }
+
+ } else {
+ /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
+
+ for (unsigned i = 0; i < 16; i++) {
+ if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+ if (i != 3) {
+ /* scale PWM out 900..2100 us to -1..1 for normal channels */
+ msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+
+ } else {
+ /* scale PWM out 900..2100 us to 0..1 for throttle */
+ msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+ }
+
+ } else {
+ /* set 0 for disabled channels */
+ msg.controls[i] = 0.0f;
+ }
+ }
+ }
+
+ msg.time_usec = hrt_absolute_time();
+ msg.mode = mavlink_base_mode;
+ msg.flags = 0;
+
+ mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
+ }
+ }
+};
class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
{
@@ -3314,6 +3467,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static),
new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static, &MavlinkStreamHILControls::get_id_static),
+ new StreamListItem(&MavlinkStreamHILActuatorControls::new_instance, &MavlinkStreamHILActuatorControls::get_name_static, &MavlinkStreamHILActuatorControls::get_id_static),
new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static),
new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static),
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static),
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index fd6fc267c6..f23efc217d 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -325,7 +325,7 @@ private:
void send_controls();
void pollForMAVLinkMessages(bool publish, int udp_port);
- void pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index);
+ void pack_actuator_message(mavlink_hil_actuator_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);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index feccfeca45..c179f71c6f 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -76,37 +76,26 @@ const unsigned mode_flag_custom = 1;
using namespace simulator;
-void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index)
+void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index)
{
- // reset state
- memset(&actuator_msg, 0, sizeof(actuator_msg));
actuator_msg.time_usec = hrt_absolute_time();
- float out[8] = {};
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
- for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
+ for (unsigned i = 0; i < MAVLINK_MSG_HIL_ACTUATOR_CONTROLS_FIELD_CONTROLS_LEN; i++) {
// scale PWM out 900..2100 us to -1..1 */
- out[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+ actuator_msg.controls[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
- if (!PX4_ISFINITE(out[i])) {
- out[i] = -1.0f;
+ if (!PX4_ISFINITE(actuator_msg.controls[i])) {
+ actuator_msg.controls[i] = -1.0f;
}
}
- actuator_msg.roll_ailerons = out[0];
- actuator_msg.pitch_elevator = out[1];
- actuator_msg.yaw_rudder = out[2];
- actuator_msg.throttle = out[3];
- actuator_msg.aux1 = out[4];
- actuator_msg.aux2 = out[5];
- actuator_msg.aux3 = out[6];
- actuator_msg.aux4 = out[7];
actuator_msg.mode = mode_flag_custom;
actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
- actuator_msg.nav_mode = index; // XXX this indicates the output group in our use of the message
+ actuator_msg.flags = 0;
}
void Simulator::send_controls()
@@ -117,9 +106,9 @@ void Simulator::send_controls()
continue;
}
- mavlink_hil_controls_t msg;
+ mavlink_hil_actuator_controls_t msg;
pack_actuator_message(msg, i);
- send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
+ send_mavlink_message(MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS, &msg, 200);
}
}
--
GitLab