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

FixedWingPositionControl: handle VEHICLE_CMD_DO_CHANGE_SPEED 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2022-03-28 11:35:04 +03:00 committed by Roman Bapst
parent 36e32ecd7b
commit ca657f36ef
2 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -257,6 +257,18 @@ FixedwingPositionControl::vehicle_command_poll()
abort_landing(true);
}
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED
&& (uint8_t)vehicle_command.param1 == vehicle_command_s::SPEED_TYPE_AIRSPEED && vehicle_command.param2 > 0) {
if (_control_mode_current == FW_POSCTRL_MODE_AUTO) {
_pos_sp_triplet.current.cruising_speed = vehicle_command.param2;
} else if (_control_mode_current == FW_POSCTRL_MODE_MANUAL_ALTITUDE
|| _control_mode_current == FW_POSCTRL_MODE_MANUAL_POSITION) {
_commanded_airspeed_setpoint = vehicle_command.param2;
}
}
}
}
@ -399,6 +411,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
(_manual_control_setpoint_airspeed * 2 - 1);
}
} else if (PX4_ISFINITE(_commanded_airspeed_setpoint)) {
altctrl_airspeed = _commanded_airspeed_setpoint;
}
return altctrl_airspeed;

1
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -266,6 +266,7 @@ private:
float _manual_control_setpoint_altitude{0.0f};
float _manual_control_setpoint_airspeed{0.0f};
float _commanded_airspeed_setpoint{(float)NAN}; ///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
hrt_abstime _time_in_fixed_bank_loiter{0};