From a2ebbe90663ce6ab7f0a4b3690ed57e503fa4267 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 27 Aug 2019 16:23:17 +0200 Subject: [PATCH] pwm_limit: rename to output_limit As there is nothing pwm-specific about it. --- src/drivers/linux_pwm_out/CMakeLists.txt | 2 +- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 28 +- src/drivers/px4fmu/CMakeLists.txt | 2 +- src/drivers/px4fmu/fmu.cpp | 1 - src/drivers/snapdragon_pwm_out/CMakeLists.txt | 2 +- .../snapdragon_pwm_out/snapdragon_pwm_out.cpp | 12 +- src/drivers/tap_esc/CMakeLists.txt | 1 - src/drivers/tap_esc/tap_esc.cpp | 1 - src/lib/CMakeLists.txt | 2 +- src/lib/mixer_module/mixer_module.cpp | 17 +- src/lib/mixer_module/mixer_module.hpp | 8 +- .../CMakeLists.txt | 2 +- .../output_limit.cpp} | 99 +- .../output_limit.h} | 36 +- src/modules/px4iofirmware/CMakeLists.txt | 2 +- src/modules/px4iofirmware/mixer.cpp | 8 +- src/modules/px4iofirmware/px4io.c | 6 +- src/modules/px4iofirmware/px4io.h | 4 +- src/modules/systemlib/param/param_new.c | 1321 +++++++++++++++++ src/modules/vtol_att_control/CMakeLists.txt | 2 - src/systemcmds/tests/CMakeLists.txt | 2 +- src/systemcmds/tests/test_mixer.cpp | 28 +- 22 files changed, 1445 insertions(+), 141 deletions(-) rename src/lib/{pwm_limit => output_limit}/CMakeLists.txt (97%) rename src/lib/{pwm_limit/pwm_limit.cpp => output_limit/output_limit.cpp} (64%) rename src/lib/{pwm_limit/pwm_limit.h => output_limit/output_limit.h} (74%) create mode 100644 src/modules/systemlib/param/param_new.c diff --git a/src/drivers/linux_pwm_out/CMakeLists.txt b/src/drivers/linux_pwm_out/CMakeLists.txt index 6b37798848..5d19e2ffc3 100644 --- a/src/drivers/linux_pwm_out/CMakeLists.txt +++ b/src/drivers/linux_pwm_out/CMakeLists.txt @@ -42,6 +42,6 @@ px4_add_module( ocpoc_mmap.cpp bbblue_pwm_rc.cpp DEPENDS - pwm_limit + output_limit ) diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index c8112dd9a0..2e6db9ce33 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include #include "common.h" @@ -98,7 +98,7 @@ px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uint32_t _groups_required = 0; uint32_t _groups_subscribed = 0; -pwm_limit_t _pwm_limit; +output_limit_t _pwm_limit; // esc parameters int32_t _pwm_disarmed; @@ -268,7 +268,7 @@ void task_main(int argc, char *argv[]) _armed.armed = false; _armed.prearmed = false; - pwm_limit_init(&_pwm_limit); + output_limit_init(&_pwm_limit); while (!_task_should_exit) { @@ -332,7 +332,7 @@ void task_main(int argc, char *argv[]) } /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; + _pwm_limit.state = OUTPUT_LIMIT_STATE_ON; } if (_mixer_group != nullptr) { @@ -358,16 +358,16 @@ void task_main(int argc, char *argv[]) uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS]; // TODO FIXME: pre-armed seems broken - pwm_limit_calc(_armed.armed, - false/*_armed.prearmed*/, - _outputs.noutputs, - reverse_mask, - disarmed_pwm, - min_pwm, - max_pwm, - _outputs.output, - pwm, - &_pwm_limit); + output_limit_calc(_armed.armed, + false/*_armed.prearmed*/, + _outputs.noutputs, + reverse_mask, + disarmed_pwm, + min_pwm, + max_pwm, + _outputs.output, + pwm, + &_pwm_limit); if (_armed.lockdown || _armed.manual_lockdown) { pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs); diff --git a/src/drivers/px4fmu/CMakeLists.txt b/src/drivers/px4fmu/CMakeLists.txt index 19e16dbbf7..d57c0148cf 100644 --- a/src/drivers/px4fmu/CMakeLists.txt +++ b/src/drivers/px4fmu/CMakeLists.txt @@ -41,5 +41,5 @@ px4_add_module( circuit_breaker mixer mixer_module - pwm_limit + output_limit ) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6bd6ae63b5..cf0b8817df 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -53,7 +53,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/snapdragon_pwm_out/CMakeLists.txt b/src/drivers/snapdragon_pwm_out/CMakeLists.txt index cf0a14f77e..6aed921c7a 100644 --- a/src/drivers/snapdragon_pwm_out/CMakeLists.txt +++ b/src/drivers/snapdragon_pwm_out/CMakeLists.txt @@ -38,5 +38,5 @@ px4_add_module( snapdragon_pwm_out.cpp DEPENDS mixer - pwm_limit + output_limit ) diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index 5e38fed10f..d642a2c5fd 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include /* @@ -105,7 +105,7 @@ px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uint32_t _groups_required = 0; // limit for pwm -pwm_limit_t _pwm_limit; +output_limit_t _pwm_limit; // esc parameters int32_t _pwm_disarmed; @@ -364,7 +364,7 @@ void task_main(int argc, char *argv[]) _armed.prearmed = false; // set max min pwm - pwm_limit_init(&_pwm_limit); + output_limit_init(&_pwm_limit); _perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency"); @@ -439,9 +439,9 @@ void task_main(int argc, char *argv[]) // TODO FIXME: pre-armed seems broken -> copied and pasted from pwm_out_rc_in: needs to be tested - pwm_limit_calc(_armed.armed, - false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, - min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); + output_limit_calc(_armed.armed, + false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, + min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); // send and publish outputs if (_armed.lockdown || _armed.manual_lockdown || timeout) { diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt index fc77f26cea..41eb096fb5 100644 --- a/src/drivers/tap_esc/CMakeLists.txt +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( tap_esc_common.cpp DEPENDS mixer - pwm_limit ) diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 3a66f3e57b..c4fee63e43 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -60,7 +60,6 @@ #include #include #include -#include #include "tap_esc_common.h" diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 259cdb7262..5da80dbf90 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -52,9 +52,9 @@ add_subdirectory(led) add_subdirectory(mathlib) add_subdirectory(mixer) add_subdirectory(mixer_module) +add_subdirectory(output_limit) add_subdirectory(perf) add_subdirectory(pid) -add_subdirectory(pwm_limit) add_subdirectory(rc) add_subdirectory(systemlib) add_subdirectory(terrain_estimation) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 9f2218c9bd..20b195521c 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -51,9 +51,8 @@ _support_esc_calibration(support_esc_calibration), _interface(interface), _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) { - /* initialize PWM limit lib */ - pwm_limit_init(&_pwm_limit); - _pwm_limit.ramp_up = ramp_up; + output_limit_init(&_output_limit); + _output_limit.ramp_up = ramp_up; /* Safely initialize armed flags */ _armed.armed = false; @@ -252,8 +251,8 @@ bool MixingOutput::update() /* except thrust to maximum. */ _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; + /* Switch off the output limit ramp for the calibration. */ + _output_limit.state = OUTPUT_LIMIT_STATE_ON; } } } @@ -262,11 +261,11 @@ bool MixingOutput::update() float outputs[MAX_ACTUATORS] {}; const unsigned mixed_num_outputs = _mixers->mix(outputs, MAX_ACTUATORS); - /* the PWM limit call takes care of out of band errors, NaN and constrains */ + /* the output limit call takes care of out of band errors, NaN and constrains */ uint16_t output_limited[MAX_ACTUATORS] {}; - pwm_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask, - _disarmed_value, _min_value, _max_value, outputs, output_limited, &_pwm_limit); + output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask, + _disarmed_value, _min_value, _max_value, outputs, output_limited, &_output_limit); /* overwrite outputs in case of force_failsafe with _failsafe_value values */ if (_armed.force_failsafe) { @@ -387,7 +386,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 } /* motor spinup phase - lock throttle to zero */ - if (output->_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) { if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 518c516e8b..f0704e9bb6 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include #include @@ -177,8 +177,8 @@ private: Command _command; ///< incoming commands (from another thread) /** - * Reorder PWM outputs according to _param_mot_ordering - * @param values PWM values to reorder + * Reorder outputs according to _param_mot_ordering + * @param values values to reorder */ inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]); @@ -192,7 +192,7 @@ private: uint16_t _min_value[MAX_ACTUATORS] {}; uint16_t _max_value[MAX_ACTUATORS] {}; uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction - pwm_limit_t _pwm_limit; + output_limit_t _output_limit; uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _safety_sub{ORB_ID(safety)}; diff --git a/src/lib/pwm_limit/CMakeLists.txt b/src/lib/output_limit/CMakeLists.txt similarity index 97% rename from src/lib/pwm_limit/CMakeLists.txt rename to src/lib/output_limit/CMakeLists.txt index 3e2c83f2f4..851872e10d 100644 --- a/src/lib/pwm_limit/CMakeLists.txt +++ b/src/lib/output_limit/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(pwm_limit pwm_limit.cpp) +px4_add_library(output_limit output_limit.cpp) diff --git a/src/lib/pwm_limit/pwm_limit.cpp b/src/lib/output_limit/output_limit.cpp similarity index 64% rename from src/lib/pwm_limit/pwm_limit.cpp rename to src/lib/output_limit/output_limit.cpp index 08c72bbf4b..781c069b52 100644 --- a/src/lib/pwm_limit/pwm_limit.cpp +++ b/src/lib/output_limit/output_limit.cpp @@ -31,16 +31,7 @@ * ****************************************************************************/ -/** - * @file pwm_limit.c - * - * Library for PWM output limiting - * - * @author Julian Oes - * @author Lorenz Meier - */ - -#include "pwm_limit.h" +#include "output_limit.h" #include #include @@ -50,21 +41,21 @@ #define PROGRESS_INT_SCALING 10000 -void pwm_limit_init(pwm_limit_t *limit) +void output_limit_init(output_limit_t *limit) { - limit->state = PWM_LIMIT_STATE_INIT; + limit->state = OUTPUT_LIMIT_STATE_INIT; limit->time_armed = 0; limit->ramp_up = true; } -void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, + const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output, + const float *output, uint16_t *effective_output, output_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { - case PWM_LIMIT_STATE_INIT: + case OUTPUT_LIMIT_STATE_INIT: if (armed) { @@ -74,19 +65,19 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c } if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = OUTPUT_LIMIT_STATE_OFF; } } break; - case PWM_LIMIT_STATE_OFF: + case OUTPUT_LIMIT_STATE_OFF: if (armed) { if (limit->ramp_up) { - limit->state = PWM_LIMIT_STATE_RAMP; + limit->state = OUTPUT_LIMIT_STATE_RAMP; } else { - limit->state = PWM_LIMIT_STATE_ON; + limit->state = OUTPUT_LIMIT_STATE_ON; } /* reset arming time, used for ramp timing */ @@ -95,19 +86,19 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c break; - case PWM_LIMIT_STATE_RAMP: + case OUTPUT_LIMIT_STATE_RAMP: if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = OUTPUT_LIMIT_STATE_OFF; } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { - limit->state = PWM_LIMIT_STATE_ON; + limit->state = OUTPUT_LIMIT_STATE_ON; } break; - case PWM_LIMIT_STATE_ON: + case OUTPUT_LIMIT_STATE_ON: if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = OUTPUT_LIMIT_STATE_OFF; } break; @@ -126,22 +117,22 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c unsigned local_limit_state = limit->state; if (pre_armed) { - local_limit_state = PWM_LIMIT_STATE_ON; + local_limit_state = OUTPUT_LIMIT_STATE_ON; } unsigned progress; - /* then set effective_pwm based on state */ + /* then set effective_output based on state */ switch (local_limit_state) { - case PWM_LIMIT_STATE_OFF: - case PWM_LIMIT_STATE_INIT: + case OUTPUT_LIMIT_STATE_OFF: + case OUTPUT_LIMIT_STATE_INIT: for (unsigned i = 0; i < num_channels; i++) { - effective_pwm[i] = disarmed_pwm[i]; + effective_output[i] = disarmed_output[i]; } break; - case PWM_LIMIT_STATE_RAMP: { + case OUTPUT_LIMIT_STATE_RAMP: { hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; @@ -156,49 +147,49 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c /* check for invalid / disabled channels */ if (!PX4_ISFINITE(control_value)) { - effective_pwm[i] = disarmed_pwm[i]; + effective_output[i] = disarmed_output[i]; continue; } - uint16_t ramp_min_pwm; + uint16_t ramp_min_output; - /* if a disarmed pwm value was set, blend between disarmed and min */ - if (disarmed_pwm[i] > 0) { + /* if a disarmed output value was set, blend between disarmed and min */ + if (disarmed_output[i] > 0) { /* safeguard against overflows */ - unsigned disarmed = disarmed_pwm[i]; + unsigned disarmed = disarmed_output[i]; - if (disarmed > min_pwm[i]) { - disarmed = min_pwm[i]; + if (disarmed > min_output[i]) { + disarmed = min_output[i]; } - unsigned disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; + unsigned disarmed_min_diff = min_output[i] - disarmed; + ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; } else { - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; + /* no disarmed output value set, choose min output */ + ramp_min_output = min_output[i]; } if (reverse_mask & (1 << i)) { control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2; + effective_output[i] = control_value * (max_output[i] - ramp_min_output) / 2 + (max_output[i] + ramp_min_output) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < ramp_min_pwm) { - effective_pwm[i] = ramp_min_pwm; + if (effective_output[i] < ramp_min_output) { + effective_output[i] = ramp_min_output; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; + } else if (effective_output[i] > max_output[i]) { + effective_output[i] = max_output[i]; } } } break; - case PWM_LIMIT_STATE_ON: + case OUTPUT_LIMIT_STATE_ON: for (unsigned i = 0; i < num_channels; i++) { @@ -206,7 +197,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c /* check for invalid / disabled channels */ if (!PX4_ISFINITE(control_value)) { - effective_pwm[i] = disarmed_pwm[i]; + effective_output[i] = disarmed_output[i]; continue; } @@ -214,14 +205,14 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + effective_output[i] = control_value * (max_output[i] - min_output[i]) / 2 + (max_output[i] + min_output[i]) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < min_pwm[i]) { - effective_pwm[i] = min_pwm[i]; + if (effective_output[i] < min_output[i]) { + effective_output[i] = min_output[i]; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; + } else if (effective_output[i] > max_output[i]) { + effective_output[i] = max_output[i]; } } diff --git a/src/lib/pwm_limit/pwm_limit.h b/src/lib/output_limit/output_limit.h similarity index 74% rename from src/lib/pwm_limit/pwm_limit.h rename to src/lib/output_limit/output_limit.h index 0d6d327fcd..be9d969bd1 100644 --- a/src/lib/pwm_limit/pwm_limit.h +++ b/src/lib/output_limit/output_limit.h @@ -32,15 +32,14 @@ ****************************************************************************/ /** - * @file pwm_limit.c + * @file output_limit.h * - * Library for PWM output limiting + * Library for output limiting (PWM for example) * * @author Julian Oes */ -#ifndef PWM_LIMIT_H_ -#define PWM_LIMIT_H_ +#pragma once #include #include @@ -49,7 +48,7 @@ __BEGIN_DECLS /* * time for the ESCs to initialize - * (this is not actually needed if PWM is sent right after boot) + * (this is not actually needed if the signal is sent right after boot) */ #define INIT_TIME_US 50000 /* @@ -57,26 +56,25 @@ __BEGIN_DECLS */ #define RAMP_TIME_US 500000 -enum pwm_limit_state { - PWM_LIMIT_STATE_OFF = 0, - PWM_LIMIT_STATE_INIT, - PWM_LIMIT_STATE_RAMP, - PWM_LIMIT_STATE_ON +enum output_limit_state { + OUTPUT_LIMIT_STATE_OFF = 0, + OUTPUT_LIMIT_STATE_INIT, + OUTPUT_LIMIT_STATE_RAMP, + OUTPUT_LIMIT_STATE_ON }; typedef struct { - enum pwm_limit_state state; + enum output_limit_state state; uint64_t time_armed; - bool ramp_up; ///< if true, motors will ramp up from disarmed to min_pwm after arming -} pwm_limit_t; + bool ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming +} output_limit_t; -__EXPORT void pwm_limit_init(pwm_limit_t *limit); +__EXPORT void output_limit_init(output_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, - const uint16_t reverse_mask, const uint16_t *disarmed_pwm, - const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, + const uint16_t reverse_mask, const uint16_t *disarmed_output, + const uint16_t *min_output, const uint16_t *max_output, + const float *output, uint16_t *effective_output, output_limit_t *limit); __END_DECLS -#endif /* PWM_LIMIT_H_ */ diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 6dcb90f817..be337d0858 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -52,5 +52,5 @@ target_link_libraries(px4iofirmware mixer rc perf - pwm_limit + output_limit ) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1177635860..0a0a1ee55b 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include @@ -311,8 +311,8 @@ mixer_tick(void) mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, - r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { @@ -482,7 +482,7 @@ mixer_callback(uintptr_t handle, } /* motor spinup phase - lock throttle to zero */ - if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { + if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 54e9ba12e3..427fc4e7ec 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -55,7 +55,7 @@ #include #include -#include +#include #include @@ -68,7 +68,7 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -pwm_limit_t pwm_limit; +output_limit_t pwm_limit; float dt; @@ -338,7 +338,7 @@ user_start(int argc, char *argv[]) syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ - pwm_limit_init(&pwm_limit); + output_limit_init(&pwm_limit); /* * P O L I C E L I G H T S diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0b3d0634d7..e5c21d0fa4 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -50,7 +50,7 @@ #include "protocol.h" -#include +#include /* hotfix: we are critically short of memory in px4io and this is the @@ -160,7 +160,7 @@ extern bool update_trims; /* * PWM limit structure */ -extern pwm_limit_t pwm_limit; +extern output_limit_t pwm_limit; /* * GPIO handling. diff --git a/src/modules/systemlib/param/param_new.c b/src/modules/systemlib/param/param_new.c new file mode 100644 index 0000000000..38d1b22afd --- /dev/null +++ b/src/modules/systemlib/param/param_new.c @@ -0,0 +1,1321 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.c + * + * Global parameter store. + * + * The implementation utilizes 2 arrays: a constant, auto-generated array + * with all parameters (param_info_base) and a dynamically resized array + * with non-default values (param_values). Both of them are sorted. + */ + +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/tinybson.h" + +#if !defined(PARAM_NO_ORB) +# include "uORB/uORB.h" +# include "uORB/topics/parameter_update.h" +#endif + +#if !defined(FLASH_BASED_PARAMS) +# define FLASH_PARAMS_EXPOSE +#else +# include "systemlib/flashparams/flashparams.h" +#endif + +#include "px4_parameters.h" +#include + + +#if 0 +# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +#ifdef __PX4_QURT +#define PARAM_OPEN px4_open +#define PARAM_CLOSE px4_close +#else +#define PARAM_OPEN open +#define PARAM_CLOSE close +#endif + +/** + * Array of static parameter info. + */ +#ifdef _UNIT_TEST +extern struct param_info_s param_array[]; +extern struct param_info_s *param_info_base; +extern struct param_info_s *param_info_limit; +#define param_info_count (param_info_limit - param_info_base) +#else +static const struct param_info_s *param_info_base = (const struct param_info_s *) &px4_parameters; +#define param_info_count px4_parameters.param_count +#endif /* _UNIT_TEST */ + +/** + * Storage for modified parameters. + */ +struct param_wbuf_s { + union param_value_u val; + param_t param; + bool unsaved; +}; + + +uint8_t *param_changed_storage = NULL; +int size_param_changed_storage_bytes = 0; +const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); + + +static inline unsigned +get_param_info_count(void) +{ + if (!param_changed_storage) { // there was an allocation failure + return 0; + } + + return param_info_count; +} + +/** flexible array holding modified parameter values. The first element is only used for + * the array size (stored in param). This simplifies atomic updates. + */ +//FLASH_PARAMS_EXPOSE UT_array *param_values; // TODO: change... +FLASH_PARAMS_EXPOSE atomic_ptr param_values = (atomic_ptr)NULL; ///< type is struct param_wbuf_s * + + +#if !defined(PARAM_NO_ORB) +/** parameter update topic handle */ +static orb_advert_t param_topic = NULL; +#endif + +static void param_set_used_internal(param_t param); + +static param_t param_find_internal(const char *name, bool notification); + +/* parameter locking uses RCU: readers don't need to lock, they just increase an atomic counter + * for the duration of the access, which makes reading wait-free and thus very efficient. + * Writers use a semaphore to protect against concurrent writes. At the same time they need + * to ensure that a reader sees a consistent state at **all** times (to simplify this, there + * is only a single pointer that is updated (param_values) which is visible to the reader). + */ +static px4_sem_t +param_sem_writer; ///< this protects against concurrent write access to param_values and param import/export +static atomic_int param_reader_counter; ///< atomic counter which a reader increases during a read access. +///< a non-zero counter signals a writer that the param_values array is still in use + +/** lock the parameter store (write access) */ +static void +param_lock_writer(void) +{ + do {} while (px4_sem_wait(¶m_sem_writer) != 0); +} + +/** unlock the parameter store (write access) */ +static void +param_unlock_writer(void) +{ + px4_sem_post(¶m_sem_writer); +} + +/** assert that the parameter store is held by a reader */ +static void +param_assert_locked_reader(void) +{ + // TODO: debug only + int reader_counter = atomic_int_load(¶m_reader_counter); + + if (reader_counter <= 0) { + //PX4_ERR("wrong reader counter!"); + // param show -c triggers this! + } +} + +void +param_init(void) +{ + px4_sem_init(¶m_sem_writer, 0, 1); + atomic_int_store(¶m_reader_counter, 0); + + /* Singleton creation of an array of bits to track changed values */ + size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit) + 1; + param_changed_storage = calloc(size_param_changed_storage_bytes, 1); +} + +/** + * Test whether a param_t is value. + * + * @param param The parameter handle to test. + * @return True if the handle is valid. + */ +static inline bool +handle_in_range(param_t param) +{ + unsigned count = get_param_info_count(); + return (count && param < count); +} + +/** + * Compare two modifid parameter structures to determine ordering. + * + * This function is suitable for passing to qsort or bsearch. + */ +static int +param_compare_values(const void *a, const void *b) +{ + struct param_wbuf_s *pa = (struct param_wbuf_s *)a; + struct param_wbuf_s *pb = (struct param_wbuf_s *)b; + + if (pa->param < pb->param) { + return -1; + } + + if (pa->param > pb->param) { + return 1; + } + + return 0; +} + +/** + * Locate the modified parameter structure for a parameter, if it exists. + * + * @param param The parameter being searched. + * @return The structure holding the modified value, or + * NULL if the parameter has not been modified. + */ +static struct param_wbuf_s * +param_find_changed(param_t param) +{ + struct param_wbuf_s *s = NULL; + + param_assert_locked_reader(); + + struct param_wbuf_s *param_values_ptr = (struct param_wbuf_s *)atomic_ptr_load(¶m_values); + + if (param_values_ptr != NULL) { + struct param_wbuf_s key; + key.param = param; + s = bsearch(&key, param_values_ptr + 1, param_values_ptr[0].param, sizeof(struct param_wbuf_s), param_compare_values); + } + + return s; +} + +static void +_param_notify_changes(bool is_saved) +{ +#if !defined(PARAM_NO_ORB) + struct parameter_update_s pup = { + .timestamp = hrt_absolute_time(), + .saved = is_saved + }; + + /* + * If we don't have a handle to our topic, create one now; otherwise + * just publish. + * + * We have a race condition here: it can happen that we call orb_advertise multiple times. + * But it will return the same handle, thus we can live with it. + */ + if (param_topic == NULL) { + param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + + } else { + orb_publish(ORB_ID(parameter_update), param_topic, &pup); + } + +#endif +} + +void +param_notify_changes(void) +{ + _param_notify_changes(true); +} + +param_t +param_find_internal(const char *name, bool notification) +{ + param_t middle; + param_t front = 0; + param_t last = get_param_info_count(); + + /* perform a binary search of the known parameters */ + + while (front <= last) { + middle = front + (last - front) / 2; + int ret = strcmp(name, param_info_base[middle].name); + + if (ret == 0) { + if (notification) { + param_set_used_internal(middle); + } + + return middle; + + } else if (middle == front) { + /* An end point has been hit, but there has been no match */ + break; + + } else if (ret < 0) { + last = middle; + + } else { + front = middle; + } + } + + /* not found */ + return PARAM_INVALID; +} + +param_t +param_find(const char *name) +{ + return param_find_internal(name, true); +} + +param_t +param_find_no_notification(const char *name) +{ + return param_find_internal(name, false); +} + +unsigned +param_count(void) +{ + return get_param_info_count(); +} + +unsigned +param_count_used(void) +{ + unsigned count = 0; + + // ensure the allocation has been done + if (get_param_info_count()) { + + for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { + if (param_changed_storage[i] & (1 << j)) { + count++; + } + } + } + } + + return count; +} + +param_t +param_for_index(unsigned index) +{ + unsigned count = get_param_info_count(); + + if (count && index < count) { + return (param_t)index; + } + + return PARAM_INVALID; +} + +param_t +param_for_used_index(unsigned index) +{ + int count = get_param_info_count(); + + if (count && index < count) { + /* walk all params and count used params */ + unsigned used_count = 0; + + for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { + if (param_changed_storage[i] & (1 << j)) { + + /* we found the right used count, + * return the param value + */ + if (index == used_count) { + return (param_t)(i * bits_per_allocation_unit + j); + } + + used_count++; + } + } + } + } + + return PARAM_INVALID; +} + +int +param_get_index(param_t param) +{ + if (handle_in_range(param)) { + return (unsigned)param; + } + + return -1; +} + +int +param_get_used_index(param_t param) +{ + /* this tests for out of bounds and does a constant time lookup */ + if (!param_used(param)) { + return -1; + } + + /* walk all params and count, now knowing that it has a valid index */ + int used_count = 0; + + for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { + if (param_changed_storage[i] & (1 << j)) { + + if ((unsigned)param == i * bits_per_allocation_unit + j) { + return used_count; + } + + used_count++; + } + } + } + + return -1; +} + +const char * +param_name(param_t param) +{ + return handle_in_range(param) ? param_info_base[param].name : NULL; +} + +bool +param_value_is_default(param_t param) +{ + struct param_wbuf_s *s; + atomic_int_fetch_and_add(¶m_reader_counter, 1); + s = param_find_changed(param); + atomic_int_fetch_and_sub(¶m_reader_counter, 1); + return s ? false : true; +} + +bool +param_value_unsaved(param_t param) +{ + struct param_wbuf_s *s; + atomic_int_fetch_and_add(¶m_reader_counter, 1); + s = param_find_changed(param); + bool ret = s && s->unsaved; + atomic_int_fetch_and_sub(¶m_reader_counter, 1); + return ret; +} + +enum param_type_e +param_type(param_t param) { + return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN; +} + +size_t +param_size(param_t param) +{ + if (handle_in_range(param)) { + + switch (param_type(param)) { + + case PARAM_TYPE_INT32: + case PARAM_TYPE_FLOAT: + return 4; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + /* decode structure size from type value */ + return param_type(param) - PARAM_TYPE_STRUCT; + + default: + return 0; + } + } + + return 0; +} + + +/** + * Obtain a pointer to the storage allocated for a parameter. + * + * @param param The parameter whose storage is sought. + * @return A pointer to the parameter value, or NULL + * if the parameter does not exist. + */ +static const void * +param_get_value_ptr(param_t param) +{ + const void *result = NULL; + + param_assert_locked_reader(); + + if (handle_in_range(param)) { + + const union param_value_u *v; + + /* work out whether we're fetching the default or a written value */ + struct param_wbuf_s *s = param_find_changed(param); + + if (s != NULL) { + v = &s->val; + + } else { + v = ¶m_info_base[param].val; + } + + if (param_type(param) >= PARAM_TYPE_STRUCT && + param_type(param) <= PARAM_TYPE_STRUCT_MAX) { + + result = v->p; + + } else { + result = v; + } + } + + return result; +} + +int +param_get(param_t param, void *val) +{ + int result = -1; + + atomic_int_fetch_and_add(¶m_reader_counter, 1); + + const void *v = param_get_value_ptr(param); + + if (val && v) { + memcpy(val, v, param_size(param)); + result = 0; + } + + atomic_int_fetch_and_sub(¶m_reader_counter, 1); + + return result; +} + +static int +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) +{ + int result = -1; + bool params_changed = false; + + param_lock_writer(); + + if (handle_in_range(param)) { + + atomic_int_fetch_and_add(¶m_reader_counter, 1); // only needed for the assertion... + struct param_wbuf_s *s = param_find_changed(param); + atomic_int_fetch_and_sub(¶m_reader_counter, 1); + + if (s == NULL) { + + params_changed = true; + + // the following is tricky: we need to insert a new element into a sorted array, + // and guarantee that readers see a consistent state at all times. + // This is where RCU comes into play: we do that by making a copy of the array, + // update the copy, then atomically replace the array. Finally we wait until we + // are sure that no reader accesses the previous array, so that it's safe to delete it. + + struct param_wbuf_s *param_values_ptr = (struct param_wbuf_s *)atomic_ptr_load(¶m_values); + int prev_param_count = 0; + + if (param_values_ptr) { + prev_param_count = param_values_ptr[0].param; + } + + struct param_wbuf_s *new_param_values = (struct param_wbuf_s *)malloc(sizeof(struct param_wbuf_s) * + (prev_param_count + 2)); + + if (new_param_values == NULL) { + PX4_ERR("alloc failed"); + goto out; + } + + if (param_values_ptr) { + memcpy(new_param_values, param_values_ptr, sizeof(struct param_wbuf_s) * (prev_param_count + 1)); + } + + new_param_values[0].param = prev_param_count + 1; // update the count + + // add the new element + int new_element_index = prev_param_count + 1; + new_param_values[new_element_index].param = param; + new_param_values[new_element_index].val.p = NULL; + new_param_values[new_element_index].unsaved = false; + + // move it to the correct place by swapping neighbors (the rest of the array is already sorted) + while (new_element_index > 1 && + param_compare_values(&new_param_values[new_element_index - 1], &new_param_values[new_element_index]) == 1) { + struct param_wbuf_s tmp; + memcpy(&tmp, &new_param_values[new_element_index], sizeof(struct param_wbuf_s)); + memcpy(&new_param_values[new_element_index], &new_param_values[new_element_index - 1], sizeof(struct param_wbuf_s)); + memcpy(&new_param_values[new_element_index - 1], &tmp, sizeof(struct param_wbuf_s)); + // TODO: only a single memcpy (keep separate new element buffer. test with param set CBRK_BUZZER ... + --new_element_index; + } + + s = &new_param_values[new_element_index]; + +#if 1 // debug: check correctness of ordering + + for (int test_index = 0; test_index < new_param_values[0].param - 1; ++test_index) { + if (new_param_values[test_index + 1].param >= new_param_values[test_index + 2].param) { + PX4_ERR("wrong order (%i %i)", (int)new_param_values[test_index + 1].param, + (int)new_param_values[test_index + 2].param); + } + } + + if (s->param != param) { + PX4_ERR("s is set wrong"); + } + +#endif + + + /* add it to the array and sort */ +// utarray_push_back(param_values, &buf); +// utarray_sort(param_values, param_compare_values); // inefficient: N log N + // qsort((param_values)->d = base pointer, (param_values)->i = num elements, sizeof(struct param_wbuf_s), param_compare_values); + + /* find it after sorting */ +// s = param_find_changed(param); //inefficient too... + + + + // the order is important: first apply the new parameter array to make it visible to new readers. + // then wait until there is no reader, which means it's safe to delete the previous array. + atomic_ptr_store(¶m_values, new_param_values); + + if (param_values_ptr) { + while (atomic_int_load(¶m_reader_counter) > 0) { + // usually we don't get here, since reader accesses are quick and do not happen that often. + usleep(1); + } + + free(param_values_ptr); + } + + } + + /* update the changed value */ + switch (param_type(param)) { + + case PARAM_TYPE_INT32: + params_changed = params_changed || s->val.i != *(int32_t *)val; + s->val.i = *(int32_t *)val; // TODO: this should be atomic (atomic read above too) + /// -> change the type + // http://stackoverflow.com/questions/35226128/are-c-c-fundamental-types-atomic + break; + + case PARAM_TYPE_FLOAT: + params_changed = params_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON; + s->val.f = *(float *)val; + // need assert: sizeof(float) == sizeof(int32) ? + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (s->val.p == NULL) { + s->val.p = malloc(param_size(param)); + + if (s->val.p == NULL) { + debug("failed to allocate parameter storage"); + goto out; + } + } + + // FIXME: this update is not atomic. we could do the memcpy into a separate buffer and then + // atomically replace the pointer. But since PARAM_TYPE_STRUCT is not used currently, we leave + // it as is. + memcpy(s->val.p, val, param_size(param)); + params_changed = true; + break; + + default: + goto out; + } + + s->unsaved = !mark_saved; + result = 0; + } + +out: + param_unlock_writer(); + + /* + * If we set something, now that we have unlocked, go ahead and advertise that + * a thing has been set. + */ + if (params_changed && notify_changes) { + _param_notify_changes(is_saved); + } + + return result; +} + +#if defined(FLASH_BASED_PARAMS) +int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) +{ + return param_set_internal(param, val, mark_saved, notify_changes, is_saved); +} + +const void *param_get_value_ptr_external(param_t param) +{ + return param_get_value_ptr(param); +} +#endif + +int +param_set(param_t param, const void *val) +{ + return param_set_internal(param, val, false, true, false); +} + +int +param_set_no_autosave(param_t param, const void *val) +{ + return param_set_internal(param, val, false, true, true); +} + +int +param_set_no_notification(param_t param, const void *val) +{ + return param_set_internal(param, val, false, false, false); +} + +bool +param_used(param_t param) +{ + int param_index = param_get_index(param); + + if (param_index < 0) { + return false; + } + + return param_changed_storage[param_index / bits_per_allocation_unit] & + (1 << param_index % bits_per_allocation_unit); +} + +void param_set_used_internal(param_t param) +{ + int param_index = param_get_index(param); + + if (param_index < 0) { + return; + } + + // TODO: atomic... + param_changed_storage[param_index / bits_per_allocation_unit] |= + (1 << param_index % bits_per_allocation_unit); +} + +int +param_reset(param_t param) +{ + struct param_wbuf_s *s = NULL; + bool param_found = false; + + param_lock_writer(); + + if (handle_in_range(param)) { + + /* look for a saved value */ + atomic_int_fetch_and_add(¶m_reader_counter, 1); // only needed for the assertion... + s = param_find_changed(param); + atomic_int_fetch_and_sub(¶m_reader_counter, 1); + + // TODO: RCU + /* if we found one, erase it */ + if (s != NULL) { + //int pos = utarray_eltidx(param_values, s); + //utarray_erase(param_values, pos, 1); + // -> use memmove + } + + param_found = true; + } + + param_unlock_writer(); + + if (s != NULL) { + _param_notify_changes(false); + } + + return (!param_found); +} + +void +param_reset_all(void) +{ + param_lock_writer(); + + struct param_wbuf_s *param_values_ptr = (struct param_wbuf_s *)atomic_ptr_load(¶m_values); + + if (param_values_ptr != NULL) { + atomic_ptr_store(¶m_values, NULL); + + // TODO: create a method... + while (atomic_int_load(¶m_reader_counter) > 0) { + // usually we don't get here, since reader accesses are quick and do not happen that often. + usleep(1); + } + + free(param_values_ptr); + } + + param_unlock_writer(); + + _param_notify_changes(false); +} + +void +param_reset_excludes(const char *excludes[], int num_excludes) +{ + param_t param; + + for (param = 0; handle_in_range(param); param++) { + const char *name = param_name(param); + bool exclude = false; + + for (int index = 0; index < num_excludes; index ++) { + int len = strlen(excludes[index]); + + if ((excludes[index][len - 1] == '*' + && strncmp(name, excludes[index], len - 1) == 0) + || strcmp(name, excludes[index]) == 0) { + exclude = true; + break; + } + } + + if (!exclude) { + param_reset(param); + } + } + + _param_notify_changes(false); +} + +static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; +static char *param_user_file = NULL; + +int +param_set_default_file(const char *filename) +{ + if (param_user_file != NULL) { + // we assume this is not in use by some other thread + free(param_user_file); + param_user_file = NULL; + } + + if (filename) { + param_user_file = strdup(filename); + } + + return 0; +} + +const char * +param_get_default_file(void) +{ + return (param_user_file != NULL) ? param_user_file : param_default_file; +} + +int +param_save_default(void) +{ + int res; +#if !defined(FLASH_BASED_PARAMS) + int fd; + + const char *filename = param_get_default_file(); + + /* write parameters to temp file */ + fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); + + if (fd < 0) { + warn("failed to open param file: %s", filename); + return ERROR; + } + + res = 1; + int attempts = 5; + + while (res != OK && attempts > 0) { + res = param_export(fd, false); + attempts--; + } + + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); + } + + PARAM_CLOSE(fd); +#else + param_lock_writer(); + res = flash_param_save(); + param_unlock_writer(); +#endif + return res; +} + +/** + * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed + */ +int +param_load_default(void) +{ + int res = 0; +#if !defined(FLASH_BASED_PARAMS) + int fd_load = PARAM_OPEN(param_get_default_file(), O_RDONLY); + + if (fd_load < 0) { + /* no parameter file is OK, otherwise this is an error */ + if (errno != ENOENT) { + warn("open '%s' for reading failed", param_get_default_file()); + return -1; + } + + return 1; + } + + int result = param_load(fd_load); + PARAM_CLOSE(fd_load); + + if (result != 0) { + warn("error reading parameters from '%s'", param_get_default_file()); + return -2; + } + +#else + // no need for locking + res = flash_param_load(); +#endif + return res; +} + +static void +param_bus_lock(bool lock) +{ + +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + + // FMUv4 has baro and FRAM on the same bus, + // as this offers on average a 100% silent + // bus for the baro operation + + // XXX this would be the preferred locking method + // if (dev == nullptr) { + // dev = px4_spibus_initialize(PX4_SPI_BUS_BARO); + // } + + // SPI_LOCK(dev, lock); + + // we lock like this for Pixracer for now + + static irqstate_t irq_state = 0; + + if (lock) { + irq_state = px4_enter_critical_section(); + + } else { + px4_leave_critical_section(irq_state); + } + +#endif +} + +int +param_export(int fd, bool only_unsaved) +{ + struct param_wbuf_s *s = NULL; + struct bson_encoder_s encoder; + int result = -1; + + param_lock_writer(); + + param_bus_lock(true); + bson_encoder_init_file(&encoder, fd); + param_bus_lock(false); + + struct param_wbuf_s *param_values_ptr = (struct param_wbuf_s *)atomic_ptr_load(¶m_values); + + /* no modified parameters -> we are done */ + if (param_values_ptr == NULL) { + result = 0; + goto out; + } + + for (int param_idx = 0; param_idx < param_values_ptr[0].param; ++param_idx) { + s = ¶m_values_ptr[param_idx + 1]; + + int32_t i; + float f; + + /* + * If we are only saving values changed since last save, and this + * one hasn't, then skip it + */ + if (only_unsaved && !s->unsaved) { + continue; + } + + s->unsaved = false; + + /* append the appropriate BSON type object */ + + + switch (param_type(s->param)) { + + case PARAM_TYPE_INT32: { + i = s->val.i; + const char *name = param_name(s->param); + + /* lock as short as possible */ + param_bus_lock(true); + + if (bson_encoder_append_int(&encoder, name, i)) { + param_bus_lock(false); + debug("BSON append failed for '%s'", name); + goto out; + } + } + break; + + case PARAM_TYPE_FLOAT: { + + f = s->val.f; + const char *name = param_name(s->param); + + /* lock as short as possible */ + param_bus_lock(true); + + if (bson_encoder_append_double(&encoder, name, f)) { + param_bus_lock(false); + debug("BSON append failed for '%s'", name); + goto out; + } + } + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: { + + const char *name = param_name(s->param); + const size_t size = param_size(s->param); + const void *value_ptr = param_get_value_ptr(s->param); + + /* lock as short as possible */ + param_bus_lock(true); + + if (bson_encoder_append_binary(&encoder, + name, + BSON_BIN_BINARY, + size, + value_ptr)) { + param_bus_lock(false); + debug("BSON append failed for '%s'", name); + goto out; + } + } + break; + + default: + debug("unrecognized parameter type"); + goto out; + } + + param_bus_lock(false); + + /* allow this process to be interrupted by another process / thread */ + usleep(5); + } + + result = 0; + +out: + param_unlock_writer(); + + if (result == 0) { + result = bson_encoder_fini(&encoder); + } + + return result; +} + +struct param_import_state { + bool mark_saved; +}; + +static int +param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + float f; + int32_t i; + void *v, *tmp = NULL; + int result = -1; + struct param_import_state *state = (struct param_import_state *)private; + + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + debug("end of parameters"); + return 0; + } + + /* + * Find the parameter this node represents. If we don't know it, + * ignore the node. + */ + param_t param = param_find_no_notification(node->name); + + if (param == PARAM_INVALID) { + debug("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + /* + * Handle setting the parameter from the node + */ + + switch (node->type) { + case BSON_INT32: + if (param_type(param) != PARAM_TYPE_INT32) { + debug("unexpected type for '%s", node->name); + goto out; + } + + i = node->i; + v = &i; + break; + + case BSON_DOUBLE: + if (param_type(param) != PARAM_TYPE_FLOAT) { + debug("unexpected type for '%s", node->name); + goto out; + } + + f = node->d; + v = &f; + break; + + case BSON_BINDATA: + if (node->subtype != BSON_BIN_BINARY) { + debug("unexpected subtype for '%s", node->name); + goto out; + } + + if (bson_decoder_data_pending(decoder) != param_size(param)) { + debug("bad size for '%s'", node->name); + goto out; + } + + /* XXX check actual file data size? */ + tmp = malloc(param_size(param)); + + if (tmp == NULL) { + debug("failed allocating for '%s'", node->name); + goto out; + } + + if (bson_decoder_copy_data(decoder, tmp)) { + debug("failed copying data for '%s'", node->name); + goto out; + } + + v = tmp; + break; + + default: + debug("unrecognised node type"); + goto out; + } + + if (param_set_internal(param, v, state->mark_saved, true, false)) { + debug("error setting value for '%s'", node->name); + goto out; + } + + if (tmp != NULL) { + free(tmp); + tmp = NULL; + } + + /* don't return zero, that means EOF */ + result = 1; + +out: + + if (tmp != NULL) { + free(tmp); + } + + return result; +} + +static int +param_import_internal(int fd, bool mark_saved) +{ + struct bson_decoder_s decoder; + int result = -1; + struct param_import_state state; + hrt_abstime t = hrt_absolute_time(); + + param_bus_lock(true); + + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { + debug("decoder init failed"); + param_bus_lock(false); + goto out; + } + + param_bus_lock(false); + + state.mark_saved = mark_saved; + + do { + param_bus_lock(true); + result = bson_decoder_next(&decoder); + //usleep(1); + param_bus_lock(false); + + } while (result > 0); + +out: + + PX4_WARN("load took: %i us -- ---------------", (int)hrt_elapsed_time(&t)); + + if (result < 0) { + debug("BSON error decoding parameters"); + } + + return result; +} + +int +param_import(int fd) +{ +#if !defined(FLASH_BASED_PARAMS) + return param_import_internal(fd, false); +#else + (void)fd; // unused + // no need for locking here + return flash_param_import(); +#endif +} + +int +param_load(int fd) +{ + param_reset_all(); + return param_import_internal(fd, true); +} + +void +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) +{ + param_t param; + + for (param = 0; handle_in_range(param); param++) { + + /* if requested, skip unchanged values */ + if (only_changed && (param_find_changed(param) == NULL)) { + continue; + } + + if (only_used && !param_used(param)) { + continue; + } + + func(arg, param); + } +} + +uint32_t param_hash_check(void) +{ + uint32_t param_hash = 0; + + atomic_int_fetch_and_add(¶m_reader_counter, 1); + + /* compute the CRC32 over all string param names and 4 byte values */ + for (param_t param = 0; handle_in_range(param); param++) { + if (!param_used(param)) { + continue; + } + + const char *name = param_name(param); + const void *val = param_get_value_ptr(param); + param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash); + param_hash = crc32part(val, param_size(param), param_hash); + } + + atomic_int_fetch_and_sub(¶m_reader_counter, 1); + + return param_hash; +} diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index 067b35a262..e6316b1459 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -39,7 +39,5 @@ px4_add_module( vtol_type.cpp tailsitter.cpp standard.cpp - DEPENDS - pwm_limit ) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 3c33ef4db0..b547d81e69 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -102,7 +102,7 @@ px4_add_module( DEPENDS git_ecl ecl_geo_lookup # TODO: move this - pwm_limit + output_limit version ) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 8c8db548df..40aa7830db 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include @@ -349,9 +349,9 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade bool MixerTest::mixerTest() { /* - * PWM limit structure + * Output limit structure */ - pwm_limit_t pwm_limit; + output_limit_t output_limit; bool should_arm = false; uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; @@ -372,7 +372,7 @@ bool MixerTest::mixerTest() unsigned mixed; const int jmax = 5; - pwm_limit_init(&pwm_limit); + output_limit_init(&output_limit); /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { @@ -388,8 +388,8 @@ bool MixerTest::mixerTest() should_prearm = true; mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &output_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -429,8 +429,8 @@ bool MixerTest::mixerTest() /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &output_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -473,8 +473,8 @@ bool MixerTest::mixerTest() /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &output_limit); //fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max); @@ -501,8 +501,8 @@ bool MixerTest::mixerTest() /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &output_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -538,8 +538,8 @@ bool MixerTest::mixerTest() /* mix */ mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &output_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) {