forked from Archive/PX4-Autopilot
px4io driver: don’t use PX4IO_PAGE_ACTUATORS page for actuator_controls_effective
This commit is contained in:
parent
2116966b1e
commit
e46d60ba6d
|
@ -1378,18 +1378,29 @@ PX4IO::io_publish_mixed_controls()
|
||||||
actuator_controls_effective_s controls_effective;
|
actuator_controls_effective_s controls_effective;
|
||||||
controls_effective.timestamp = hrt_absolute_time();
|
controls_effective.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
/* get actuator controls from IO */
|
// XXX PX4IO_PAGE_ACTUATORS page contains actuator outputs, not inputs
|
||||||
uint16_t act[_max_actuators];
|
// need to implement backward mixing in IO's mixed and add another page
|
||||||
int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
|
// 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)
|
//if (ret != OK)
|
||||||
return ret;
|
// return ret;
|
||||||
|
|
||||||
/* convert from register format to float */
|
/* convert from register format to float */
|
||||||
for (unsigned i = 0; i < _max_actuators; i++)
|
//for (unsigned i = 0; i < _max_actuators; i++)
|
||||||
controls_effective.control_effective[i] = REG_TO_FLOAT(act[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) {
|
if (_to_actuators_effective == 0) {
|
||||||
_to_actuators_effective =
|
_to_actuators_effective =
|
||||||
orb_advertise((_primary_pwm_device ?
|
orb_advertise((_primary_pwm_device ?
|
||||||
|
|
|
@ -185,7 +185,7 @@ mixer_tick(void)
|
||||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||||
|
|
||||||
/* safe actuators for FMU feedback */
|
/* 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++)
|
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
|
||||||
r_page_servos[i] = 0;
|
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) {
|
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
|
||||||
|
|
Loading…
Reference in New Issue