|
|
|
@ -1306,9 +1306,9 @@ PX4IO::io_set_control_state(unsigned group)
@@ -1306,9 +1306,9 @@ PX4IO::io_set_control_state(unsigned group)
|
|
|
|
|
controls.control[3] = 1.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint16_t regs[_max_actuators]; |
|
|
|
|
uint16_t regs[sizeof(controls.control) / sizeof(controls.control[0])] = {}; |
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < _max_controls; i++) { |
|
|
|
|
for (unsigned i = 0; (i < _max_controls) && (i < sizeof(controls.control) / sizeof(controls.control[0])); i++) { |
|
|
|
|
/* ensure FLOAT_TO_REG does not produce an integer overflow */ |
|
|
|
|
const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f); |
|
|
|
|
|
|
|
|
@ -1319,12 +1319,12 @@ PX4IO::io_set_control_state(unsigned group)
@@ -1319,12 +1319,12 @@ PX4IO::io_set_control_state(unsigned group)
|
|
|
|
|
regs[i] = FLOAT_TO_REG(ctrl); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!_test_fmu_fail && !_motor_test.in_test_mode) { |
|
|
|
|
/* copy values to registers in IO */ |
|
|
|
|
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); |
|
|
|
|
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, math::min(_max_controls, |
|
|
|
|
sizeof(controls.control) / sizeof(controls.control[0]))); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
return OK; |
|
|
|
|