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

mkblctrl cleanup & tested

Browse Source
This commit is contained in:
marco
2013-05-24 20:16:47 +02:00
parent 9f090e651a
commit 8e1571cf02
1 changed files with 24 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mkblctrl/mkblctrl.cpp
+24 -7
View File
@@ -491,8 +491,10 @@ MK::task_main()
* Subscribe to the appropriate PWM output topic based on whether we are the * Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not. * primary PWM output or not.
*/ */
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : // loeschen _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_0)); // loeschen ORB_ID(actuator_controls_0));
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/* force a reset of the update rate */ /* force a reset of the update rate */
_current_update_rate = 0; _current_update_rate = 0;
@@ -502,11 +504,19 @@ MK::task_main()
/* advertise the mixed control outputs */ /* advertise the mixed control outputs */
actuator_outputs_s outputs; actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(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);
/* advertise the effective control inputs */ /* advertise the effective control inputs */
actuator_controls_effective_s controls_effective; actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective)); memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */ /* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2]; pollfd fds[2];
fds[0].fd = _t_actuators; fds[0].fd = _t_actuators;
@@ -556,7 +566,8 @@ MK::task_main()
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */ /* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_0), _t_actuators, &_controls); // loeschen orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_0), _t_actuators, &_controls);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* can we mix? */ /* can we mix? */
if (_mixers != nullptr) { if (_mixers != nullptr) {
@@ -1105,13 +1116,19 @@ ssize_t
MK::write(file *filp, const char *buffer, size_t len) MK::write(file *filp, const char *buffer, size_t len)
{ {
unsigned count = len / 2; unsigned count = len / 2;
uint16_t values[4]; // loeschen uint16_t values[4];
uint16_t values[8];
if (count > 4) { // loeschen if (count > 4) {
// we only have 4 PWM outputs on the FMU // loeschen // we only have 4 PWM outputs on the FMU
count = 4; // loeschen count = 4;
// loeschen }
if (count > _num_outputs) {
// we only have 8 I2C outputs in the driver
count = _num_outputs;
} }
// allow for misaligned values // allow for misaligned values
memcpy(values, buffer, count * 2); memcpy(values, buffer, count * 2);