From 62603bee45018161e6e92481d9e6f798e9aa51c5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 21 Oct 2016 17:14:25 +0200
Subject: [PATCH] simulation: SITL outputs from 0..1
Instead of sending actuator controls from -1..1 for SITL, we should send
0..1 like we already do for HIL. This will enable negative thrust in the
future, e.g. for pusher props that spin backwards, or for vehicles with
variable pitch propellers.
---
Tools/jMAVSim | 2 +-
Tools/sitl_gazebo | 2 +-
src/modules/mavlink/mavlink_messages.cpp | 8 +-
src/modules/simulator/simulator.h | 11 ++-
src/modules/simulator/simulator_mavlink.cpp | 87 ++++++++++++++++++---
5 files changed, 93 insertions(+), 17 deletions(-)
diff --git a/Tools/jMAVSim b/Tools/jMAVSim
index 4e49f05e57..fec27e3ce5 160000
--- a/Tools/jMAVSim
+++ b/Tools/jMAVSim
@@ -1 +1 @@
-Subproject commit 4e49f05e57d9f7555259843b8081ec7e607ef49e
+Subproject commit fec27e3ce567c794b5c364dc9856b55431130181
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 380fce8b47..b9ed67482e 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 380fce8b478361b086cef18597f885002975e701
+Subproject commit b9ed67482e940124133a281da963a8239e8b574c
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 1b2484c86a..a917faf146 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2172,11 +2172,11 @@ protected:
for (unsigned i = 0; i < 8; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) {
- /* scale PWM out 900..2100 us to 0..1 for rotors */
+ /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for rotors */
out[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 */
+ /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
}
@@ -2192,11 +2192,11 @@ protected:
for (unsigned i = 0; i < 8; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
- /* scale PWM out 900..2100 us to -1..1 for normal channels */
+ /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for normal channels */
out[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 */
+ /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
}
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 812686a3a4..0e755f4b66 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -241,7 +241,8 @@ private:
_flow_pub(nullptr),
_dist_pub(nullptr),
_battery_pub(nullptr),
- _initialized(false)
+ _initialized(false),
+ _system_type(0)
#ifndef __PX4_QURT
,
_rc_channels_pub(nullptr),
@@ -257,6 +258,11 @@ private:
_vehicle_status{}
#endif
{
+ // We need to know the type for the correct mapping from
+ // actuator controls to the hil actuator message.
+ param_t param_system_type = param_find("MAV_TYPE");
+ param_get(param_system_type, &_system_type);
+
for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++)
{
_actuator_outputs_sub[i] = -1;
@@ -299,6 +305,9 @@ private:
// Lib used to do the battery calculations.
Battery _battery;
+ // For param MAV_TYPE
+ int32_t _system_type;
+
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index d23d5e973a..b8556ca0c6 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -78,26 +78,93 @@ const unsigned mode_flag_custom = 1;
using namespace simulator;
-void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index)
+void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsigned index)
{
- actuator_msg.time_usec = hrt_absolute_time();
+ msg.time_usec = hrt_absolute_time();
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 < MAVLINK_MSG_HIL_ACTUATOR_CONTROLS_FIELD_CONTROLS_LEN; i++) {
- // scale PWM out 900..2100 us to -1..1 */
- actuator_msg.controls[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+ /* 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 ||
+ _system_type == MAV_TYPE_VTOL_RESERVED2) {
- if (!PX4_ISFINITE(actuator_msg.controls[i])) {
- actuator_msg.controls[i] = -1.0f;
+ /* 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;
+
+ case MAV_TYPE_VTOL_RESERVED2:
+ n = 8;
+ break;
+
+ default:
+ n = 8;
+ break;
+ }
+
+ for (unsigned i = 0; i < 16; i++) {
+ if (_actuators[index].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);
+
+ } 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);
+ }
+
+ } 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 (_actuators[index].output[i] > PWM_DEFAULT_MIN / 2) {
+ if (i != 3) {
+ /* 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);
+
+ } 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);
+ }
+
+ } else {
+ /* set 0 for disabled channels */
+ msg.controls[i] = 0.0f;
+ }
}
}
- actuator_msg.mode = mode_flag_custom;
- actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
- actuator_msg.flags = 0;
+ msg.mode = mode_flag_custom;
+ msg.mode |= (armed) ? mode_flag_armed : 0;
+ msg.flags = 0;
}
void Simulator::send_controls()