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

added logic to encode NAN as INT16_MAX to transfer to IO from FMU

Browse Source
This commit is contained in:
dlwalter
2019-08-19 21:08:11 +02:00
committed by Lorenz Meier
parent c1c253d9be
commit 310fd80405
2 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+8 -1
View File
@@ -1304,8 +1304,15 @@ PX4IO::io_set_control_state(unsigned group)
for (unsigned i = 0; i < _max_controls; i++) {
/* ensure FLOAT_TO_REG does not produce an integer overflow */
const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f);
if(!isfinite(ctrl)){
regs[i] = INT16_MAX;
}
else
{
regs[i] = FLOAT_TO_REG(ctrl);
}
regs[i] = FLOAT_TO_REG(ctrl);
}
if (!_test_fmu_fail) {
src/modules/px4iofirmware/mixer.cpp
+7 -1
View File
@@ -412,7 +412,13 @@ mixer_callback(uintptr_t handle,
switch (source) {
case MIX_FMU:
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
if(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX){
//catch NAN values encoded as INT16 max for disarmed outputs
control = NAN;
}
else{
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
}
break;
}