From c9d72c0d0721e3a05930a683b2e1ed3bc59f8134 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 12 Mar 2018 14:41:30 +0100
Subject: [PATCH] PWMSim : add MC_AIRMODE support
---
src/drivers/pwm_out_sim/PWMSim.cpp | 30 ++++++++++++++++++++++++++++++
src/drivers/pwm_out_sim/PWMSim.hpp | 4 ++++
2 files changed, 34 insertions(+)
diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp
index 7224a494f9..ddbd49cde6 100644
--- a/src/drivers/pwm_out_sim/PWMSim.cpp
+++ b/src/drivers/pwm_out_sim/PWMSim.cpp
@@ -135,6 +135,18 @@ PWMSim::subscribe()
}
}
+void PWMSim::update_params()
+{
+ // multicopter air-mode
+ param_t param_handle = param_find("MC_AIRMODE");
+
+ if (param_handle != PARAM_INVALID) {
+ int32_t val;
+ param_get(param_handle, &val);
+ _airmode = val > 0;
+ }
+}
+
void
PWMSim::run()
{
@@ -146,6 +158,9 @@ PWMSim::run()
/* advertise the mixed control outputs, insist on the first group output */
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_actuator_outputs);
+ update_params();
+ int params_sub = orb_subscribe(ORB_ID(parameter_update));
+
/* loop until killed */
while (!should_exit()) {
@@ -172,6 +187,10 @@ PWMSim::run()
_current_update_rate = _update_rate;
}
+ if (_mixers) {
+ _mixers->set_airmode(_airmode);
+ }
+
/* this can happen during boot, but after the sleep its likely resolved */
if (_poll_fds_num == 0) {
usleep(1000 * 1000);
@@ -283,6 +302,16 @@ PWMSim::run()
_lockdown = aa.manual_lockdown;
}
}
+
+ /* check for parameter updates */
+ bool param_updated = false;
+ orb_check(params_sub, ¶m_updated);
+
+ if (param_updated) {
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), params_sub, &update);
+ update_params();
+ }
}
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
@@ -292,6 +321,7 @@ PWMSim::run()
}
orb_unsubscribe(_armed_sub);
+ orb_unsubscribe(params_sub);
}
int
diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp
index 5331ac18e5..732636d129 100644
--- a/src/drivers/pwm_out_sim/PWMSim.hpp
+++ b/src/drivers/pwm_out_sim/PWMSim.hpp
@@ -49,6 +49,7 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/parameter_update.h>
class PWMSim : public device::CDev, public ModuleBase<PWMSim>
{
@@ -129,10 +130,13 @@ private:
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
+ bool _airmode{false}; ///< multicopter air-mode
+
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
+ void update_params();
};
#endif /* DRIVERS_PWM_OUT_SIM_PWMSIM_HPP_ */
--
GitLab