diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 7dc812931945..8d1b3d090ed3 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -44,6 +44,7 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/rc_channels.h> +#include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> @@ -99,17 +100,19 @@ int32_t _pwm_max; MixerGroup *_mixer_group = nullptr; -void usage(); +static void usage(); -void start(); +static void start(); -void stop(); +static void stop(); -void task_main_trampoline(int argc, char *argv[]); +static void task_main_trampoline(int argc, char *argv[]); -void subscribe(); +static void subscribe(); -void task_main(int argc, char *argv[]); +static void task_main(int argc, char *argv[]); + +static void update_params(bool &airmode); /* mixer initialization */ int initialize_mixer(const char *mixer_filename); @@ -128,6 +131,19 @@ int mixer_control_callback(uintptr_t handle, } +void update_params(bool &airmode) +{ + // 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; + } +} + + int initialize_mixer(const char *mixer_filename) { char buf[4096]; @@ -227,6 +243,10 @@ void task_main(int argc, char *argv[]) // subscribe and set up polling subscribe(); + bool airmode = false; + update_params(airmode); + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + int rc_channels_sub = -1; // Start disarmed @@ -244,6 +264,10 @@ void task_main(int argc, char *argv[]) orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); } + if (_mixer_group) { + _mixer_group->set_airmode(airmode); + } + int pret = px4_poll(_poll_fds, _poll_fds_num, 10); /* Timed out, do a periodic check for _task_should_exit. */ @@ -366,6 +390,17 @@ void task_main(int argc, char *argv[]) PX4_ERR("Could not mix output! Exiting..."); _task_should_exit = true; } + + /* 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(airmode); + } + } delete pwm_out; @@ -385,6 +420,10 @@ void task_main(int argc, char *argv[]) orb_unsubscribe(rc_channels_sub); } + if (params_sub != -1) { + orb_unsubscribe(params_sub); + } + _is_running = false; } diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 7224a494f998..ddbd49cde6c1 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 5331ac18e596..732636d129ff 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 @@ class PWMSim : public device::CDev, public ModuleBase<PWMSim> 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_ */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index fa83c751e0ed..4c2ac5ddded7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -275,6 +275,7 @@ class PX4FMU : public device::CDev, public ModuleBase<PX4FMU> float _mot_t_max; // maximum rise time for motor (slew rate limiting) float _thr_mdl_fac; // thrust to pwm modelling factor + bool _airmode; // multicopter air-mode perf_counter_t _ctl_latency; @@ -385,6 +386,7 @@ PX4FMU::PX4FMU(bool run_as_task) : _to_mixer_status(nullptr), _mot_t_max(0.0f), _thr_mdl_fac(0.0f), + _airmode(false), _ctl_latency(perf_alloc(PC_ELAPSED, "ctl_lat")) { for (unsigned i = 0; i < _max_actuators; i++) { @@ -1360,6 +1362,8 @@ PX4FMU::cycle() ORB_PRIO_DEFAULT); } + _mixers->set_airmode(_airmode); + perf_end(_ctl_latency); } } @@ -1783,6 +1787,16 @@ void PX4FMU::update_params() if (param_handle != PARAM_INVALID) { param_get(param_handle, &_thr_mdl_fac); } + + // multicopter air-mode + param_handle = param_find("MC_AIRMODE"); + + if (param_handle != PARAM_INVALID) { + int32_t val; + param_get(param_handle, &val); + _airmode = val > 0; + PX4_DEBUG("%s: %d", "MC_AIRMODE", _airmode); + } } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ce913cf4c309..164d798c2d3f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1221,6 +1221,15 @@ PX4IO::task_main() param_get(parm_handle, ¶m_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_MOTOR_SLEW_MAX, FLOAT_TO_REG(param_val)); } + + /* air-mode */ + parm_handle = param_find("MC_AIRMODE"); + + if (parm_handle != PARAM_INVALID) { + int32_t param_val_int; + param_get(parm_handle, ¶m_val_int); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val_int)); + } } } diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index 7727cd2a6cf4..04b9f1602768 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -44,6 +44,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> +#include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> @@ -117,23 +118,25 @@ MultirotorMixer *_mixer = nullptr; * forward declaration */ -void usage(); +static void usage(); -void start(); +static void start(); -void stop(); +static void stop(); -int pwm_initialize(const char *device); +static int pwm_initialize(const char *device); -void pwm_deinitialize(); +static void pwm_deinitialize(); -void send_outputs_pwm(const uint16_t *pwm); +static void send_outputs_pwm(const uint16_t *pwm); -void task_main_trampoline(int argc, char *argv[]); +static void task_main_trampoline(int argc, char *argv[]); -void subscribe(); +static void subscribe(); -void task_main(int argc, char *argv[]); +static void task_main(int argc, char *argv[]); + +static void update_params(bool &airmode); int initialize_mixer(const char *mixer_filename); @@ -157,6 +160,17 @@ int mixer_control_callback(uintptr_t handle, return 0; } +void update_params(bool &airmode) +{ + // 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; + } +} int initialize_mixer(const char *mixer_filename) { @@ -343,6 +357,10 @@ void task_main(int argc, char *argv[]) // subscribe and set up polling subscribe(); + bool airmode = false; + update_params(airmode); + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + // Start disarmed _armed.armed = false; _armed.prearmed = false; @@ -355,6 +373,10 @@ void task_main(int argc, char *argv[]) // Main loop while (!_task_should_exit) { + if (_mixer) { + _mixer->set_airmode(airmode); + } + /* wait up to 10ms for data */ int pret = px4_poll(_poll_fds, _poll_fds_num, 10); @@ -438,6 +460,15 @@ void task_main(int argc, char *argv[]) _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); } + /* 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(airmode); + } } pwm_deinitialize(); @@ -449,6 +480,7 @@ void task_main(int argc, char *argv[]) } orb_unsubscribe(_armed_sub); + orb_unsubscribe(params_sub); _is_running = false; } diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 989a59e43ffd..bf7203a19277 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -218,6 +218,13 @@ class __EXPORT Mixer */ virtual void set_thrust_factor(float val) {} + /** + * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors. + * + * @param[in] airmode De/-activate airmode by setting it to false/true + */ + virtual void set_airmode(bool airmode) {}; + protected: /** client-supplied callback used when fetching control values */ ControlCallback _control_cb; @@ -403,6 +410,8 @@ class __EXPORT MixerGroup : public Mixer */ virtual void set_thrust_factor(float val); + virtual void set_airmode(bool airmode); + private: Mixer *_first; /**< linked list of mixers */ @@ -650,6 +659,8 @@ class __EXPORT MultirotorMixer : public Mixer */ virtual void set_thrust_factor(float val) {_thrust_factor = val;} + virtual void set_airmode(bool airmode); + union saturation_status { struct { uint16_t valid : 1; // 0 - true when the saturation status is used @@ -675,6 +686,8 @@ class __EXPORT MultirotorMixer : public Mixer float _delta_out_max; float _thrust_factor; + bool _airmode; + void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low); saturation_status _saturation_status; diff --git a/src/lib/mixer/mixer_group.cpp b/src/lib/mixer/mixer_group.cpp index e4fb98fbff7c..200be683baf4 100644 --- a/src/lib/mixer/mixer_group.cpp +++ b/src/lib/mixer/mixer_group.cpp @@ -185,7 +185,17 @@ MixerGroup::set_thrust_factor(float val) mixer->set_thrust_factor(val); mixer = mixer->_next; } +} + +void +MixerGroup::set_airmode(bool airmode) +{ + Mixer *mixer = _first; + while (mixer != nullptr) { + mixer->set_airmode(airmode); + mixer = mixer->_next; + } } uint16_t diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 8bd087d9a29d..4463ea38fba2 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -84,6 +84,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ _delta_out_max(0.0f), _thrust_factor(0.0f), + _airmode(false), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]), _outputs_prev(new float[_rotor_count]) @@ -179,10 +180,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) // clean out class variable used to capture saturation _saturation_status.value = 0; - // thrust boost parameters - float thrust_increase_factor = 1.5f; - float thrust_decrease_factor = 0.6f; - /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -203,48 +200,38 @@ MultirotorMixer::mix(float *outputs, unsigned space) float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch - - // Note: thrust boost is computed assuming thrust_gain==1 for all motors. - // On asymmetric platforms, some motors have thrust_gain<1, + float delta_out_max = max_out - min_out; // distance between the two extrema + + // If the difference between the to extrema is smaller than 1.0, the boost can safely unsaturate a motor if needed + // without saturating another one. + // Otherwise, a scaler is computed to make the distance between the two extrema exacly 1.0 and the boost + // value is computed to maximize the roll-pitch control. + // + // Note: thrust boost is computed assuming thrust_scale==1 for all motors. + // On asymmetric platforms, some motors have thrust_scale<1, // which may result in motor saturation after thrust boost is applied // TODO: revise the saturation/boosting strategy - - if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { - float max_thrust_diff = thrust * thrust_increase_factor - thrust; - - if (max_thrust_diff >= -min_out) { + if (delta_out_max <= 1.0f) { + if (min_out < 0.0f) { boost = -min_out; - } else { - boost = max_thrust_diff; - roll_pitch_scale = (thrust + boost) / (thrust - min_out); - } - - } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor * thrust; - - if (max_thrust_diff >= max_out - 1.0f) { + } else if (max_out > 1.0f) { boost = -(max_out - 1.0f); - - } else { - boost = -max_thrust_diff; - roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } - } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { - float max_thrust_diff = thrust * thrust_increase_factor - thrust; - boost = math::constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); - roll_pitch_scale = (thrust + boost) / (thrust - min_out); - - } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor * thrust; - boost = math::constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); - roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); + } else { + roll_pitch_scale = 1.0f / (delta_out_max); + boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust); + } - } else if (min_out < 0.0f && max_out > 1.0f) { - boost = math::constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust, - thrust_increase_factor * thrust - thrust); - roll_pitch_scale = (thrust + boost) / (thrust - min_out); + if (!_airmode) { + // disable positive boosting if not in air-mode + // boosting can only be positive when min_out < 0.0 + // roll_pitch_scale is reduced accordingly + if (boost > 0.0f) { + roll_pitch_scale = thrust / (thrust - min_out); + boost = 0.0f; + } } // capture saturation @@ -447,6 +434,12 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.valid = true; } +void +MultirotorMixer::set_airmode(bool airmode) +{ + _airmode = airmode; +} + void MultirotorMixer::groups_required(uint32_t &groups) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 89202e086e4d..08f6d9f4236e 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -542,4 +542,18 @@ PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f); * @increment 10 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 30.f); +PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); + +/** + * Multicopter air-mode + * + * The air-mode enables the mixer to increase the total thrust of the multirotor + * in order to keep attitude and rate control even at low and high throttle. + * This function should be disabled during tuning as it will help the controller + * to diverge if the closed-loop is unstable. + * + * @boolean + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_INT32(MC_AIRMODE, 0); + diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5a167a447f39..b874c8cac236 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -254,6 +254,11 @@ mixer_tick(void) */ mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT); + /* + * Update air-mode parameter + */ + mixer_group.set_airmode(REG_TO_BOOL(r_setup_airmode)); + /* * Run the mixers. diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 04fd0712fc38..8e6e425e35a3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -78,6 +78,8 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + #define PX4IO_PROTOCOL_VERSION 4 /* maximum allowable sizes on this protocol version */ @@ -232,6 +234,8 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /* max motor slew rate */ +#define PX4IO_P_SETUP_AIRMODE 27 /* air-mode */ + #define PX4IO_P_SETUP_THR_MDL_FAC 25 /* factor for modelling static pwm output to thrust relationship */ #define PX4IO_P_SETUP_THERMAL 26 /* thermal management */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 00b27372a8d2..10db31a277ff 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -132,6 +132,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] #define r_setup_thr_fac r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC] #define r_setup_slew_max r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX] +#define r_setup_airmode r_page_setup[PX4IO_P_SETUP_AIRMODE] #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fc6919863579..462398cf0859 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -178,6 +178,7 @@ volatile uint16_t r_page_setup[] = { [PX4IO_P_SETUP_SCALE_PITCH] = 10000, [PX4IO_P_SETUP_SCALE_YAW] = 10000, [PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0, + [PX4IO_P_SETUP_AIRMODE] = 0, [PX4IO_P_SETUP_THR_MDL_FAC] = 0, [PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE }; @@ -704,6 +705,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) sbus1_set_output_rate_hz(value); break; + case PX4IO_P_SETUP_AIRMODE: + r_page_setup[PX4IO_P_SETUP_AIRMODE] = value; + break; + case PX4IO_P_SETUP_THR_MDL_FAC: update_mc_thrust_param = true; r_page_setup[offset] = value; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 286f3b9ed0b1..d9137b41c2c7 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -58,6 +58,7 @@ #include <arch/chip/chip.h> #include <uORB/topics/esc_status.h> +#include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> #include <drivers/drv_pwm_output.h> @@ -417,6 +418,18 @@ int UavcanNode::get_param(int remote_node_id, const char *name) return rv; } +void UavcanNode::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; + } +} + int UavcanNode::start_fw_server() { int rv = -1; @@ -512,7 +525,7 @@ int UavcanNode::fw_server(eServerAction action) int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { - warnx("Already started"); + PX4_WARN("Already started"); return -1; } @@ -546,14 +559,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) can = new CanInitHelper(); if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown - warnx("Out of memory"); + PX4_ERR("Out of memory"); return -1; } const int can_init_res = can->init(bitrate); if (can_init_res < 0) { - warnx("CAN driver init failed %i", can_init_res); + PX4_ERR("CAN driver init failed %i", can_init_res); return can_init_res; } } @@ -564,12 +577,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) _instance = new UavcanNode(can->driver, uavcan_stm32::SystemClock::instance()); if (_instance == nullptr) { - warnx("Out of memory"); - return -1; - } - - if (_instance == nullptr) { - warnx("Out of memory"); + PX4_ERR("Out of memory"); return -1; } @@ -578,7 +586,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) if (node_init_res < 0) { delete _instance; _instance = nullptr; - warnx("Node init failed %i", node_init_res); + PX4_ERR("Node init failed %i", node_init_res); return node_init_res; } @@ -590,7 +598,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) static_cast<main_t>(run_trampoline), nullptr); if (_instance->_task < 0) { - warnx("start failed: %d", errno); + PX4_ERR("start failed: %d", errno); return -errno; } @@ -610,7 +618,7 @@ void UavcanNode::fill_node_info() swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; // Too verbose for normal operation - //warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + //PX4_INFO("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); _node.setSoftwareVersion(swver); @@ -664,11 +672,11 @@ int UavcanNode::init(uavcan::NodeID node_id) ret = br->init(); if (ret < 0) { - warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); + PX4_ERR("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); return ret; } - warnx("sensor bridge '%s' init ok", br->get_name()); + PX4_INFO("sensor bridge '%s' init ok", br->get_name()); br = br->getSibling(); } @@ -683,7 +691,7 @@ void UavcanNode::node_spin_once() const int spin_res = _node.spinOnce(); if (spin_res < 0) { - warnx("node spin error %i", spin_res); + PX4_ERR("node spin error %i", spin_res); } @@ -774,7 +782,7 @@ int UavcanNode::run() const int slave_init_res = _time_sync_slave.start(); if (slave_init_res < 0) { - warnx("Failed to start time_sync_slave"); + PX4_ERR("Failed to start time_sync_slave"); _task_should_exit = true; } @@ -792,7 +800,7 @@ int UavcanNode::run() const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { - warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); + PX4_ERR("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -819,8 +827,22 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } + update_params(); + + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + while (!_task_should_exit) { + /* 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(); + } + switch (_fw_server_action) { case Start: _fw_server_status = start_fw_server(); @@ -908,6 +930,8 @@ int UavcanNode::run() // but this driver could well serve multiple groups. unsigned num_outputs_max = 8; + _mixers->set_airmode(_airmode); + // Do mixing _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); @@ -978,10 +1002,11 @@ int UavcanNode::run() } } + orb_unsubscribe(params_sub); + (void)::close(busevent_fd); teardown(); - warnx("exiting."); exit(0); } @@ -1028,12 +1053,12 @@ UavcanNode::subscribe() // the first fd used by CAN for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { - warnx("subscribe to actuator_controls_%d", i); + PX4_DEBUG("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } if (unsub_groups & (1 << i)) { - warnx("unsubscribe from actuator_controls_%d", i); + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); orb_unsubscribe(_control_subs[i]); _control_subs[i] = -1; } @@ -1096,7 +1121,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - warnx("mixer load failed with %d", ret); + PX4_ERR("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -1154,10 +1179,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) void UavcanNode::print_info() { - if (!_instance) { - warnx("not running, start first"); - } - (void)pthread_mutex_lock(&_node_mutex); // Memory status @@ -1270,10 +1291,10 @@ void UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command */ static void print_usage() { - warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n" - "\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>|\n" - "\t hardpoint set <id> <command>}"); + PX4_INFO("usage: \n" + "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n" + "\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>|\n" + "\t hardpoint set <id> <command>}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1293,7 +1314,7 @@ int uavcan_main(int argc, char *argv[]) int rv = UavcanNode::instance()->fw_server(UavcanNode::Start); if (rv < 0) { - warnx("Firmware Server Failed to Start %d", rv); + PX4_ERR("Firmware Server Failed to Start %d", rv); ::exit(rv); } @@ -1301,7 +1322,7 @@ int uavcan_main(int argc, char *argv[]) } // Already running, no error - warnx("already started"); + PX4_INFO("already started"); ::exit(0); } @@ -1310,7 +1331,7 @@ int uavcan_main(int argc, char *argv[]) (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { - warnx("Invalid Node ID %i", node_id); + PX4_ERR("Invalid Node ID %i", node_id); ::exit(1); } @@ -1319,7 +1340,7 @@ int uavcan_main(int argc, char *argv[]) (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); // Start - warnx("Node ID %u, bitrate %u", node_id, bitrate); + PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); } @@ -1449,7 +1470,7 @@ int uavcan_main(int argc, char *argv[]) inst->shrink(); if (rv < 0) { - warnx("Firmware Server Failed to Stop %d", rv); + PX4_ERR("Firmware Server Failed to Stop %d", rv); ::exit(rv); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 4c0575e5c0be..69cc732a87d5 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -152,6 +152,7 @@ class UavcanNode : public device::CDev int request_fw_check(); int print_params(uavcan::protocol::param::GetSet::Response &resp); int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req); + void update_params(); void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; @@ -207,6 +208,8 @@ class UavcanNode : public device::CDev actuator_outputs_s _outputs = {}; + bool _airmode = false; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];