fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 02:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor pwm_out_sim: use mixer_module and run on work queue

Browse Source 
Tested with SITL + HITL
This commit is contained in:
Beat Küng
2019-10-21 09:42:08 +02:00
parent 2837152983
commit 349469cf75
10 changed files with 137 additions and 572 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+24 -24
View File
@@ -123,9 +123,6 @@ public:
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static PX4FMU *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@@ -163,7 +160,10 @@ public:
unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, true};
static constexpr int FMU_MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
static_assert(FMU_MAX_ACTUATORS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails");
MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
Mode _mode{MODE_NONE};
@@ -497,7 +497,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
for (unsigned pass = 0; pass < 2; pass++) {
/* We should note that group is iterated over from 0 to MAX_ACTUATORS.
/* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS.
* This allows for the ideal worlds situation: 1 channel per group
* configuration.
*
@@ -511,7 +511,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
* rate and mode. (See rates above.)
*/
for (unsigned group = 0; group < MAX_ACTUATORS; group++) {
for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
@@ -606,7 +606,7 @@ PX4FMU::update_pwm_rev_mask()
return;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
char pname[16];
/* fill the channel reverse mask from parameters */
@@ -630,7 +630,7 @@ PX4FMU::update_pwm_trims()
return;
}
int16_t values[MAX_ACTUATORS] = {};
int16_t values[FMU_MAX_ACTUATORS] = {};
const char *pname_format;
@@ -645,7 +645,7 @@ PX4FMU::update_pwm_trims()
return;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
char pname[16];
/* fill the struct from parameters */
@@ -661,7 +661,7 @@ PX4FMU::update_pwm_trims()
}
/* copy the trim values to the mixer offsets */
unsigned n_out = _mixing_output.mixers()->set_trims(values, MAX_ACTUATORS);
unsigned n_out = _mixing_output.mixers()->set_trims(values, FMU_MAX_ACTUATORS);
PX4_DEBUG("set %d trims", n_out);
}
@@ -893,7 +893,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -926,11 +926,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.failsafeValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
break;
}
@@ -938,7 +938,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -969,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
if (_mixing_output.disarmedValue(i) > 0) {
_num_disarmed_set++;
}
@@ -981,11 +981,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.disarmedValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
break;
}
@@ -993,7 +993,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -1025,11 +1025,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.minValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
arg = (unsigned long)&pwm;
break;
}
@@ -1038,7 +1038,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -1063,11 +1063,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.maxValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
arg = (unsigned long)&pwm;
break;
}
@@ -1076,7 +1076,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
PX4_DEBUG("error: too many trim values: %d", pwm->channel_count);
ret = -EINVAL;
break;