Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New pwm subsystem supporting centralized pwm resource coordination #3

Merged
merged 1 commit into from
Sep 19, 2018
Merged

New pwm subsystem supporting centralized pwm resource coordination #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 20 additions & 7 deletions src/drivers/px4ecu/px4ecu.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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