From 01a5f7a9698840b5ca5dcebe89a38f138378194c Mon Sep 17 00:00:00 2001 From: rolandash Date: Mon, 17 Sep 2018 13:51:40 +0800 Subject: [PATCH] adjust capture. --- src/drivers/px4ecu/px4ecu.cpp | 27 ++++++--- src/drivers/px4fmu/fmu.cpp | 82 +++++++++++++++------------ src/drivers/rgbled_pwm/rgbled_pwm.cpp | 1 + src/lib/pwmgroups/pwmgroups.cpp | 29 ++++++++-- src/lib/pwmgroups/pwmgroups.h | 7 ++- 5 files changed, 98 insertions(+), 48 deletions(-) diff --git a/src/drivers/px4ecu/px4ecu.cpp b/src/drivers/px4ecu/px4ecu.cpp index ee916a6f40ec..7d4fb183a351 100644 --- a/src/drivers/px4ecu/px4ecu.cpp +++ b/src/drivers/px4ecu/px4ecu.cpp @@ -160,6 +160,7 @@ class PX4ECU : public StaticPwmDevice bool _pwm_initialized; MixerGroup *_mixers; + unsigned _mixed_num_outputs; uint32_t _groups_required; uint32_t _groups_subscribed; @@ -826,6 +827,18 @@ void PX4ECU::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { + //TODO: check if mixer present and mixed output number; + if (_mixers != nullptr) { + for (uint8_t i = 0; i < _mixed_num_outputs; i++) { + _working_channel_map |= (1 << (i + _channel_map_offset)); + } + if (_working_channel_map != _channel_rate_map) { + //mixed rates here; + _working_rate = PWM_GROUP_RATE_MIXED; + printf("[ecu] mixed group rates.\n"); + } + + } up_pwm_servo_init(_working_channel_map, 0); //up_pwm_servo_init(0x0F0F, 0); printf("[ecu] init pwm out channel alt rate map: 0x%04X\n", _pwm_alt_rate_channels); @@ -1020,7 +1033,7 @@ PX4ECU::cycle() /* do mixing */ float outputs[_max_actuators]; - size_t mixed_num_outputs = _mixers->mix(outputs, _num_outputs); + _mixed_num_outputs = _mixers->mix(outputs, _num_outputs); /* publish mixer status */ multirotor_motor_limits_s multirotor_motor_limits = {}; @@ -1039,7 +1052,7 @@ PX4ECU::cycle() /* disable unused ports by setting their output to NaN */ for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { - if (i >= mixed_num_outputs) { + if (i >= _mixed_num_outputs) { outputs[i] = NAN_VALUE; } } @@ -1047,27 +1060,27 @@ PX4ECU::cycle() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask, + pwm_limit_calc(_throttle_armed, arm_nothrottle(), _mixed_num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ if (_armed.force_failsafe) { - for (size_t i = 0; i < mixed_num_outputs; i++) { + for (size_t i = 0; i < _mixed_num_outputs; i++) { pwm_limited[i] = _failsafe_pwm[i]; } } /* overwrite outputs in case of lockdown with disarmed PWM values */ if (_armed.lockdown || _armed.manual_lockdown) { - for (size_t i = 0; i < mixed_num_outputs; i++) { + for (size_t i = 0; i < _mixed_num_outputs; i++) { pwm_limited[i] = _disarmed_pwm[i]; } } /* output to the servos */ if (_pwm_initialized) { - for (size_t i = 0; i < mixed_num_outputs; i++) { + for (size_t i = 0; i < _mixed_num_outputs; i++) { //pwm_output_set(i, pwm_limited[i]); set_pwm_channel_value_single(i, pwm_limited[i]); } @@ -1081,7 +1094,7 @@ PX4ECU::cycle() up_pwm_update(); } - publish_pwm_outputs(pwm_limited, mixed_num_outputs); + publish_pwm_outputs(pwm_limited, _mixed_num_outputs); perf_end(_ctl_latency); } } diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 57f93dc4ad11..0ac50a6b5d1b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -214,7 +214,8 @@ class PX4FMU : public StaticPwmDevice, public ModuleBase bool _pwm_initialized; MixerGroup *_mixers; - + unsigned _mixed_num_outputs; + uint32_t _groups_required; uint32_t _groups_subscribed; int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; @@ -1147,10 +1148,24 @@ void PX4FMU::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { + //TODO: check if mixer present and mixed output number; + if (_mixers != nullptr) { + for (uint8_t i = 0; i < _mixed_num_outputs; i++) { + _working_channel_map |= (1 << (i + _channel_map_offset)); + } + if (_working_channel_map != _channel_rate_map) { + //mixed rates here; + _working_rate = PWM_GROUP_RATE_MIXED; + printf("[fmu] mixed group rates.\n"); + + } + } + up_pwm_servo_init(_working_channel_map, 0); //_pwm_alt_rate_channels = _pwm_alt_rate_channels << 8; printf("[fmu] init pwm out channel alt rate map: 0x%04X\n", _pwm_alt_rate_channels); set_pwm_channel_rates (_pwm_alt_rate, _pwm_default_rate); + if (register_working_channels() != OK) { return; } @@ -1300,24 +1315,24 @@ PX4FMU::cycle() /* do mixing */ float outputs[_max_actuators]; - const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs); + _mixed_num_outputs = _mixers->mix(outputs, _num_outputs); /* the PWM limit call takes care of out of band errors, NaN and constrains */ uint16_t pwm_limited[MAX_ACTUATORS]; - pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask, + pwm_limit_calc(_throttle_armed, arm_nothrottle(), _mixed_num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ if (_armed.force_failsafe) { - for (size_t i = 0; i < mixed_num_outputs; i++) { + for (size_t i = 0; i < _mixed_num_outputs; i++) { pwm_limited[i] = _failsafe_pwm[i]; } } /* overwrite outputs in case of lockdown with disarmed PWM values */ if (_armed.lockdown || _armed.manual_lockdown) { - for (size_t i = 0; i < mixed_num_outputs; i++) { + for (size_t i = 0; i < _mixed_num_outputs; i++) { pwm_limited[i] = _disarmed_pwm[i]; } } @@ -1327,7 +1342,7 @@ PX4FMU::cycle() /* output to the servos */ if (_pwm_initialized) { - for (size_t i = 0; i < mixed_num_outputs; i++) { + for (size_t i = 0; i < _mixed_num_outputs; i++) { //pwm_output_set(i, pwm_limited[i]); set_pwm_channel_value_single(i, pwm_limited[i]); } @@ -1342,10 +1357,10 @@ PX4FMU::cycle() actuator_outputs_s actuator_outputs = {}; actuator_outputs.timestamp = hrt_absolute_time(); - actuator_outputs.noutputs = mixed_num_outputs; + actuator_outputs.noutputs = _mixed_num_outputs; // zero unused outputs - for (size_t i = 0; i < mixed_num_outputs; ++i) { + for (size_t i = 0; i < _mixed_num_outputs; ++i) { actuator_outputs.output[i] = pwm_limited[i]; } @@ -2501,9 +2516,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_SET: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; ret = up_input_capture_set(chan, pconfig->edge, pconfig->filter, pconfig->callback, pconfig->context); } @@ -2513,9 +2527,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_SET_CALLBACK: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + _capture_channel_map = 1 << chan; ret = up_input_capture_set_callback(chan, pconfig->callback, pconfig->context); } @@ -2524,9 +2537,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_GET_CALLBACK: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; ret = up_input_capture_get_callback(chan, &pconfig->callback, &pconfig->context); } @@ -2535,9 +2547,9 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_GET_STATS: if (arg) { uint8_t chan = stats->chan_in_edges_out; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; + ret = up_input_capture_get_stats(chan, stats, false); } @@ -2546,9 +2558,9 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_GET_CLR_STATS: if (arg) { uint8_t chan = stats->chan_in_edges_out; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; + ret = up_input_capture_get_stats(chan, stats, true); } @@ -2557,9 +2569,9 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_SET_EDGE: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; + ret = up_input_capture_set_trigger(chan, pconfig->edge); } @@ -2568,9 +2580,9 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_GET_EDGE: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; + ret = up_input_capture_get_trigger(chan, &pconfig->edge); } @@ -2579,9 +2591,9 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_SET_FILTER: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; + ret = up_input_capture_set_filter(chan, pconfig->filter); } @@ -2590,9 +2602,9 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg) case INPUT_CAP_GET_FILTER: if (pconfig) { uint8_t chan = pconfig->channel; -#ifdef BOARD_HAS_ECU_PWM - //chan = chan + 8; -#endif + chan = chan + _channel_offset; + //_capture_channel_map = 1 << chan; + ret = up_input_capture_get_filter(chan, &pconfig->filter); } diff --git a/src/drivers/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/rgbled_pwm/rgbled_pwm.cpp index cd053ed076a6..beeca34dd67b 100644 --- a/src/drivers/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/rgbled_pwm/rgbled_pwm.cpp @@ -157,6 +157,7 @@ RGBLED_PWM::init() _default_rate = 50; _alt_rate = 400; _working_rate = PWM_GROUP_RATE_UNSPECIFIED; + _channel_rate_map = _working_channel_map; if (register_working_channels()!=OK) { printf("fail to register rgb led\n"); diff --git a/src/lib/pwmgroups/pwmgroups.cpp b/src/lib/pwmgroups/pwmgroups.cpp index 9cf671c34f52..61e8f22ed4a5 100644 --- a/src/lib/pwmgroups/pwmgroups.cpp +++ b/src/lib/pwmgroups/pwmgroups.cpp @@ -62,6 +62,9 @@ PwmGroups::PwmGroups(const char *name, const char *devname) : _default_rate = PWM_OUTPUT_RATE_DEFAULT; _alt_rate = PWM_OUTPUT_RATE_ALT; _working_rate = PWM_GROUP_RATE_UNSPECIFIED; + _capture_channel_map = 0; + _working_channel_map = 0; + _channel_rate_map = 0; } @@ -134,6 +137,21 @@ PwmGroups::register_working_channels() { } //free channel available, check conflicts on rate; + if (_working_rate == PWM_GROUP_RATE_MIXED) { + //both alt rate and default rate at work. + uint32_t default_rate_channel_map = (~_channel_rate_map) & _working_channel_map; + if (!is_free_channels_available(_channel_rate_map, PWM_GROUP_RATE_ALT) + || !is_free_channels_available(default_rate_channel_map, PWM_GROUP_RATE_DEFAULT)) { + return -EINVAL; + } + else { + all_channels |= _working_channel_map; + update_default_alt_rate_map (PWM_GROUP_RATE_ALT, _channel_rate_map); + update_default_alt_rate_map (PWM_GROUP_RATE_DEFAULT, default_rate_channel_map); + printf("[pwmgroups] mixed rate, registered channels all: 0x%04X\n", all_channels); + return OK; + } + } //obligation to use specified rate; //TODO: static pwm device needs to specify working rate on start. move to subclass @@ -186,7 +204,7 @@ PwmGroups::register_working_channels() { _working_rate = PWM_GROUP_RATE_ALT; //check rate conflicts; - if(!is_free_channels_available(_channel_rate_map, PWM_GROUP_RATE_DEFAULT)) { + if(!is_free_channels_available(_channel_rate_map, PWM_GROUP_RATE_ALT)) { return -EINVAL; } } @@ -196,16 +214,16 @@ PwmGroups::register_working_channels() { } } - //no conflicts, allocate the channel + //no conflicts, allocate requested channels all_channels |= _working_channel_map; - update_default_alt_rate_map (_working_rate, _working_channel_map); + update_default_alt_rate_map (_working_rate, _channel_rate_map); printf("[pwmgroups] registered channels all: 0x%04X\n", all_channels); return OK; } void PwmGroups::update_default_alt_rate_map (pwm_groups_rate_t rate_type, uint32_t channel_map) { - + //TODO: add critical section protection; if (rate_type == PWM_GROUP_RATE_ALT) { _alt_rate_channels |= channel_map; //remove corresponding bit in opposite rate map if its there; @@ -221,7 +239,7 @@ PwmGroups::update_default_alt_rate_map (pwm_groups_rate_t rate_type, uint32_t ch } } else { - printf("[pwmgroups] selected default reate\n"); + printf("[pwmgroups] selected default rate\n"); _default_rate_channels |= channel_map; //printf("[pwmgroups] offset: %d, default channel 0x%04X\n", _channel_map_offset, _default_rate_channels); @@ -232,6 +250,7 @@ PwmGroups::update_default_alt_rate_map (pwm_groups_rate_t rate_type, uint32_t ch } } } + printf("[pwmgroups] device name: %s\n", get_devname()); printf("[pwmgroups] occupation channel map: 0x%04X\n", channel_map); printf("[pwmgroups] alt rate channel: 0x%04X default rate channel: 0x%04X\n", _alt_rate_channels, _default_rate_channels); printf("[pwmgroups] alt rate timer: 0x%04X, default rate timer: 0x%04X\n", _alt_rate_timers, _default_rate_timers); diff --git a/src/lib/pwmgroups/pwmgroups.h b/src/lib/pwmgroups/pwmgroups.h index fdc537b3fbc7..a2d23cbe8665 100644 --- a/src/lib/pwmgroups/pwmgroups.h +++ b/src/lib/pwmgroups/pwmgroups.h @@ -119,6 +119,7 @@ typedef enum { PWM_GROUP_RATE_ALT, PWM_GROUP_RATE_DEFAULT, + PWM_GROUP_RATE_MIXED, PWM_GROUP_RATE_UNSPECIFIED } pwm_groups_rate_t; @@ -224,9 +225,13 @@ class __EXPORT PwmGroups : public device::CDev //group channels must be continously allocated; uint32_t _group_channel_map; uint32_t _working_channel_map; - //map of channels with alt rate; + //map of channels with alt rate, different from _working_channel_map in some cases, + //e.g., a mixer group includes both main output and camera trigger; uint32_t _channel_rate_map; + //channel map for capture function; + uint32_t _capture_channel_map; + uint8_t _timer_map_offset; uint32_t _channel_map_offset;