dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io driver: don’t use PX4IO_PAGE_ACTUATORS page for actuator_controls_effective

This commit is contained in:
Anton Babushkin 2013-11-15 11:32:05 +04:00
parent 2116966b1e
commit e46d60ba6d
2 changed files with 24 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/drivers/px4io/px4io.cpp
View File

@ -1378,18 +1378,29 @@ PX4IO::io_publish_mixed_controls()
actuator_controls_effective_s controls_effective;
controls_effective.timestamp = hrt_absolute_time();
/* get actuator controls from IO */
uint16_t act[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
// XXX PX4IO_PAGE_ACTUATORS page contains actuator outputs, not inputs
// need to implement backward mixing in IO's mixed and add another page
// by now simply use control inputs here
/* get actuator outputs from IO */
//uint16_t act[_max_actuators];
//int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
if (ret != OK)
return ret;
//if (ret != OK)
// return ret;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
//for (unsigned i = 0; i < _max_actuators; i++)
// controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
/* laxily advertise on first publication */
actuator_controls_s controls;
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &controls);
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = controls.control[i];
/* lazily advertise on first publication */
if (_to_actuators_effective == 0) {
_to_actuators_effective =
orb_advertise((_primary_pwm_device ?

6
src/modules/px4iofirmware/mixer.cpp
View File

@ -185,7 +185,7 @@ mixer_tick(void)
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
@ -201,6 +201,10 @@ mixer_tick(void)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {