aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/px4fmu/fmu.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/px4fmu/fmu.cpp')
-rw-r--r--src/drivers/px4fmu/fmu.cpp351
1 files changed, 245 insertions, 106 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 0fbd84924..0a4635728 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -92,6 +92,7 @@ public:
MODE_2PWM,
MODE_4PWM,
MODE_6PWM,
+ MODE_8PWM,
};
PX4FMU();
virtual ~PX4FMU();
@@ -113,6 +114,9 @@ private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
static const unsigned _max_actuators = 6;
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ static const unsigned _max_actuators = 8;
+#endif
Mode _mode;
unsigned _pwm_default_rate;
@@ -120,19 +124,25 @@ private:
uint32_t _pwm_alt_rate_channels;
unsigned _current_update_rate;
int _task;
- int _t_actuators;
- int _t_actuator_armed;
- orb_advert_t _t_outputs;
+ int _armed_sub;
+ orb_advert_t _outputs_pub;
+ actuator_armed_s _armed;
unsigned _num_outputs;
bool _primary_pwm_device;
volatile bool _task_should_exit;
- bool _armed;
+ bool _servo_armed;
bool _pwm_on;
MixerGroup *_mixers;
- actuator_controls_s _controls;
+ uint32_t _groups_required;
+ uint32_t _groups_subscribed;
+ int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
+ actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
+ orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
+ pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+ unsigned _poll_fds_num;
pwm_limit_t _pwm_limit;
uint16_t _failsafe_pwm[_max_actuators];
@@ -143,13 +153,13 @@ private:
unsigned _num_disarmed_set;
static void task_main_trampoline(int argc, char *argv[]);
- void task_main() __attribute__((noreturn));
+ void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
-
+ void subscribe();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@@ -197,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_VDD_5V_HIPOWER_OC, 0, 0},
{GPIO_VDD_5V_PERIPH_OC, 0, 0},
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ /* AeroCore breaks out User GPIOs on J11 */
+ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+ {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+ {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
+ {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
+ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
+ {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0},
+ {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0},
+ {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0},
+ {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0},
+ {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0},
+ {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0},
+#endif
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -216,25 +240,36 @@ PX4FMU::PX4FMU() :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
- _t_actuators(-1),
- _t_actuator_armed(-1),
- _t_outputs(0),
+ _armed_sub(-1),
+ _outputs_pub(-1),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
- _armed(false),
+ _servo_armed(false),
_pwm_on(false),
_mixers(nullptr),
- _failsafe_pwm({0}),
- _disarmed_pwm({0}),
- _num_failsafe_set(0),
- _num_disarmed_set(0)
+ _groups_required(0),
+ _groups_subscribed(0),
+ _control_subs{-1},
+ _poll_fds_num(0),
+ _failsafe_pwm{0},
+ _disarmed_pwm{0},
+ _num_failsafe_set(0),
+ _num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
_max_pwm[i] = PWM_DEFAULT_MAX;
}
+ _control_topics[0] = ORB_ID(actuator_controls_0);
+ _control_topics[1] = ORB_ID(actuator_controls_1);
+ _control_topics[2] = ORB_ID(actuator_controls_2);
+ _control_topics[3] = ORB_ID(actuator_controls_3);
+
+ memset(_controls, 0, sizeof(_controls));
+ memset(_poll_fds, 0, sizeof(_poll_fds));
+
_debug_enabled = true;
}
@@ -365,6 +400,20 @@ PX4FMU::set_mode(Mode mode)
break;
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
+ debug("MODE_8PWM");
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0xff);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+ break;
+#endif
+
case MODE_NONE:
debug("MODE_NONE");
@@ -448,32 +497,42 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
}
void
+PX4FMU::subscribe()
+{
+ /* subscribe/unsubscribe to required actuator control groups */
+ uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+ uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+ _poll_fds_num = 0;
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (sub_groups & (1 << i)) {
+ warnx("subscribe to actuator_controls_%d", i);
+ _control_subs[i] = orb_subscribe(_control_topics[i]);
+ }
+ if (unsub_groups & (1 << i)) {
+ warnx("unsubscribe from actuator_controls_%d", i);
+ ::close(_control_subs[i]);
+ _control_subs[i] = -1;
+ }
+
+ if (_control_subs[i] > 0) {
+ _poll_fds[_poll_fds_num].fd = _control_subs[i];
+ _poll_fds[_poll_fds_num].events = POLLIN;
+ _poll_fds_num++;
+ }
+ }
+}
+
+void
PX4FMU::task_main()
{
- /*
- * Subscribe to the appropriate PWM output topic based on whether we are the
- * primary PWM output or not.
- */
- _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
- ORB_ID(actuator_controls_1));
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
+ _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
- /* advertise the mixed control outputs */
- _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
- &outputs);
-
- pollfd fds[2];
- fds[0].fd = _t_actuators;
- fds[0].events = POLLIN;
- fds[1].fd = _t_actuator_armed;
- fds[1].events = POLLIN;
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
@@ -491,6 +550,12 @@ PX4FMU::task_main()
/* loop until killed */
while (!_task_should_exit) {
+ if (_groups_subscribed != _groups_required) {
+ subscribe();
+ _groups_subscribed = _groups_required;
+ /* force setting update rate */
+ _current_update_rate = 0;
+ }
/*
* Adjust actuator topic update rate to keep up with
@@ -515,20 +580,23 @@ PX4FMU::task_main()
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
- orb_set_interval(_t_actuators, update_rate_in_ms);
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ orb_set_interval(_control_subs[i], update_rate_in_ms);
+ }
+ }
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, stopping to check for PPM
- * input at 100Hz */
- int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
+ * input at 50Hz */
+ int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
- usleep(1000000);
continue;
} else if (ret == 0) {
@@ -537,89 +605,100 @@ PX4FMU::task_main()
} else {
- /* do we have a control update? */
- if (fds[0].revents & POLLIN) {
+ /* get controls for required topics */
+ unsigned poll_id = 0;
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ if (_poll_fds[poll_id].revents & POLLIN) {
+ orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+ }
+ poll_id++;
+ }
+ }
- /* get controls - must always do this to avoid spinning */
- orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+ /* can we mix? */
+ if (_mixers != nullptr) {
- /* can we mix? */
- if (_mixers != nullptr) {
+ unsigned num_outputs;
- unsigned num_outputs;
+ switch (_mode) {
+ case MODE_2PWM:
+ num_outputs = 2;
+ break;
- switch (_mode) {
- case MODE_2PWM:
- num_outputs = 2;
- break;
+ case MODE_4PWM:
+ num_outputs = 4;
+ break;
- case MODE_4PWM:
- num_outputs = 4;
- break;
+ case MODE_6PWM:
+ num_outputs = 6;
+ break;
- case MODE_6PWM:
- num_outputs = 6;
- break;
+ case MODE_8PWM:
+ num_outputs = 8;
+ break;
+ default:
+ num_outputs = 0;
+ break;
+ }
- default:
- num_outputs = 0;
- break;
+ /* do mixing */
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.timestamp = hrt_absolute_time();
+
+ /* iterate actuators */
+ for (unsigned i = 0; i < num_outputs; i++) {
+ /* last resort: catch NaN and INF */
+ if ((i >= outputs.noutputs) ||
+ !isfinite(outputs.output[i])) {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ outputs.output[i] = -1.0f;
}
+ }
- /* do mixing */
- outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
- outputs.timestamp = hrt_absolute_time();
-
- /* iterate actuators */
- for (unsigned i = 0; i < num_outputs; i++) {
- /* last resort: catch NaN, INF and out-of-band errors */
- if (i >= outputs.noutputs ||
- !isfinite(outputs.output[i]) ||
- outputs.output[i] < -1.0f ||
- outputs.output[i] > 1.0f) {
- /*
- * Value is NaN, INF or out of band - set to the minimum value.
- * This will be clearly visible on the servo status and will limit the risk of accidentally
- * spinning motors. It would be deadly in flight.
- */
- outputs.output[i] = -1.0f;
- }
- }
+ uint16_t pwm_limited[num_outputs];
- uint16_t pwm_limited[num_outputs];
+ /* the PWM limit call takes care of out of band errors and constrains */
+ pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
- pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+ /* output to the servos */
+ for (unsigned i = 0; i < num_outputs; i++) {
+ up_pwm_servo_set(i, pwm_limited[i]);
+ }
- /* output to the servos */
- for (unsigned i = 0; i < num_outputs; i++) {
- up_pwm_servo_set(i, pwm_limited[i]);
- }
+ /* publish mixed control outputs */
+ if (_outputs_pub < 0) {
+ _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs);
+ } else {
- /* and publish for anyone that cares to see */
- orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
}
}
+ }
- /* how about an arming update? */
- if (fds[1].revents & POLLIN) {
- actuator_armed_s aa;
+ /* check arming state */
+ bool updated = false;
+ orb_check(_armed_sub, &updated);
- /* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+ if (updated) {
+ orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
- /* update the armed status and check that we're not locked down */
- bool set_armed = aa.armed && !aa.lockdown;
+ /* update the armed status and check that we're not locked down */
+ bool set_armed = _armed.armed && !_armed.lockdown;
- if (_armed != set_armed)
- _armed = set_armed;
+ if (_servo_armed != set_armed)
+ _servo_armed = set_armed;
- /* update PWM status if armed or if disarmed PWM values are set */
- bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+ /* update PWM status if armed or if disarmed PWM values are set */
+ bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
- if (_pwm_on != pwm_on) {
- _pwm_on = pwm_on;
- up_pwm_servo_arm(pwm_on);
- }
+ if (_pwm_on != pwm_on) {
+ _pwm_on = pwm_on;
+ up_pwm_servo_arm(pwm_on);
}
}
@@ -661,8 +740,13 @@ PX4FMU::task_main()
}
- ::close(_t_actuators);
- ::close(_t_actuator_armed);
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+ if (_control_subs[i] > 0) {
+ ::close(_control_subs[i]);
+ _control_subs[i] = -1;
+ }
+ }
+ ::close(_armed_sub);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -684,7 +768,7 @@ PX4FMU::control_callback(uintptr_t handle,
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
- input = controls->control[control_index];
+ input = controls[control_group].control[control_index];
return 0;
}
@@ -707,6 +791,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_2PWM:
case MODE_4PWM:
case MODE_6PWM:
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case MODE_8PWM:
+#endif
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -736,6 +823,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
+ case PWM_SERVO_SET_FORCE_SAFETY_OFF:
// these are no-ops, as no safety switch
break;
@@ -935,6 +1023,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case PWM_SERVO_SET(7):
+ case PWM_SERVO_SET(6):
+ if (_mode < MODE_8PWM) {
+ ret = -EINVAL;
+ break;
+ }
+#endif
+
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
@@ -962,6 +1059,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case PWM_SERVO_GET(7):
+ case PWM_SERVO_GET(6):
+ if (_mode < MODE_8PWM) {
+ ret = -EINVAL;
+ break;
+ }
+#endif
+
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) {
@@ -989,12 +1095,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_RATEGROUP(3):
case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5):
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case PWM_SERVO_GET_RATEGROUP(6):
+ case PWM_SERVO_GET_RATEGROUP(7):
+#endif
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case MODE_8PWM:
+ *(unsigned *)arg = 8;
+ break;
+#endif
+
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
@@ -1040,6 +1156,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_6PWM);
break;
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ case 8:
+ set_mode(MODE_8PWM);
+ break;
+#endif
default:
ret = -EINVAL;
@@ -1052,6 +1173,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
+ _groups_required = 0;
}
break;
@@ -1060,18 +1182,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
- (uintptr_t)&_controls, mixinfo);
+ (uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
+ _groups_required = 0;
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
- (uintptr_t)&_controls);
+ (uintptr_t)_controls);
_mixers->add_mixer(mixer);
+ _mixers->groups_required(_groups_required);
}
break;
@@ -1082,9 +1206,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
- _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+ _mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
if (_mixers == nullptr) {
+ _groups_required = 0;
ret = -ENOMEM;
} else {
@@ -1095,7 +1220,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
debug("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
+ _groups_required = 0;
ret = -EINVAL;
+ } else {
+
+ _mixers->groups_required(_groups_required);
}
}
@@ -1122,10 +1251,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
unsigned count = len / 2;
uint16_t values[6];
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ if (count > 8) {
+ // we have at most 8 outputs
+ count = 8;
+ }
+#else
if (count > 6) {
// we have at most 6 outputs
count = 6;
}
+#endif
// allow for misaligned values
memcpy(values, buffer, count * 2);
@@ -1400,6 +1536,9 @@ fmu_new_mode(PortMode new_mode)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
servo_mode = PX4FMU::MODE_6PWM;
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ servo_mode = PX4FMU::MODE_8PWM;
+#endif
break;
/* mixed modes supported on v1 board only */
@@ -1714,10 +1853,10 @@ fmu_main(int argc, char *argv[])
}
- fprintf(stderr, "FMU: unrecognised command, try:\n");
+ fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-17 14:01:04 +0000