Skip to content

Commit

Permalink
Merge pull request #3 from airmind/new_pwm_group
Browse files Browse the repository at this point in the history 
New pwm subsystem supporting centralized pwm resource coordination
  • Loading branch information
@rolandash
rolandash authored Sep 19, 2018
2 parents 1d41cc9 + 01a5f7a commit ca0893d
Show file tree
Hide file tree
Showing 5 changed files with 98 additions and 48 deletions.
27 changes: 20 additions & 7 deletions src/drivers/px4ecu/px4ecu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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 = {};
Expand All @@ -1039,35 +1052,35 @@ 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;
}
}

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]);
}
Expand All @@ -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);
}
}
Expand Down
82 changes: 47 additions & 35 deletions src/drivers/px4fmu/fmu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,8 @@ class PX4FMU : public StaticPwmDevice, public ModuleBase<PX4FMU>
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];
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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];
}
}
Expand All @@ -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]);
}
Expand All @@ -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];
}

Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand Down
1 change: 1 addition & 0 deletions src/drivers/rgbled_pwm/rgbled_pwm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
29 changes: 24 additions & 5 deletions src/lib/pwmgroups/pwmgroups.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
}
Expand All @@ -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;
Expand All @@ -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);

Expand All @@ -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);
Expand Down
7 changes: 6 additions & 1 deletion src/lib/pwmgroups/pwmgroups.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down

0 comments on commit ca0893d

Please sign in to comment.