mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-11 22:21:36 +08:00
fw_att_control rename fw_att_control_main.cpp to match class
This commit is contained in:
Daniel Agar
@@ -0,0 +1,935 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "FixedwingAttitudeControl.hpp"
|
||||
|
||||
/**
|
||||
* Fixedwing attitude control app start / stop handling function
|
||||
*
|
||||
* @ingroup apps
|
||||
*/
|
||||
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
|
||||
|
||||
FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
SuperBlock(nullptr, "FW_ATT"),
|
||||
/* subscriptions */
|
||||
_att_sub(-1),
|
||||
_att_sp_sub(-1),
|
||||
_battery_status_sub(-1),
|
||||
_global_pos_sub(-1),
|
||||
_manual_sub(-1),
|
||||
_params_sub(-1),
|
||||
_vcontrol_mode_sub(-1),
|
||||
_vehicle_land_detected_sub(-1),
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_rate_sp_pub(nullptr),
|
||||
_attitude_sp_pub(nullptr),
|
||||
_actuators_0_pub(nullptr),
|
||||
_actuators_2_pub(nullptr),
|
||||
_rate_ctrl_status_pub(nullptr),
|
||||
|
||||
_rates_sp_id(nullptr),
|
||||
_actuators_id(nullptr),
|
||||
_attitude_setpoint_id(nullptr),
|
||||
|
||||
_sub_airspeed(ORB_ID(airspeed), 0, 0, &getSubscriptions()),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
|
||||
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")),
|
||||
/* states */
|
||||
_flaps_applied(0),
|
||||
_flaperons_applied(0),
|
||||
_roll(0.0f),
|
||||
_pitch(0.0f),
|
||||
_yaw(0.0f)
|
||||
{
|
||||
/* safely initialize structs */
|
||||
_actuators = {};
|
||||
_actuators_airframe = {};
|
||||
_att = {};
|
||||
_att_sp = {};
|
||||
_battery_status = {};
|
||||
_global_pos = {};
|
||||
_manual = {};
|
||||
_rates_sp = {};
|
||||
_vcontrol_mode = {};
|
||||
_vehicle_land_detected = {};
|
||||
_vehicle_status = {};
|
||||
|
||||
_parameter_handles.p_tc = param_find("FW_P_TC");
|
||||
_parameter_handles.p_p = param_find("FW_PR_P");
|
||||
_parameter_handles.p_i = param_find("FW_PR_I");
|
||||
_parameter_handles.p_ff = param_find("FW_PR_FF");
|
||||
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
|
||||
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
|
||||
_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
|
||||
|
||||
_parameter_handles.r_tc = param_find("FW_R_TC");
|
||||
_parameter_handles.r_p = param_find("FW_RR_P");
|
||||
_parameter_handles.r_i = param_find("FW_RR_I");
|
||||
_parameter_handles.r_ff = param_find("FW_RR_FF");
|
||||
_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
|
||||
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
|
||||
|
||||
_parameter_handles.y_p = param_find("FW_YR_P");
|
||||
_parameter_handles.y_i = param_find("FW_YR_I");
|
||||
_parameter_handles.y_ff = param_find("FW_YR_FF");
|
||||
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
|
||||
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
|
||||
_parameter_handles.roll_to_yaw_ff = param_find("FW_RLL_TO_YAW_FF");
|
||||
|
||||
_parameter_handles.w_en = param_find("FW_W_EN");
|
||||
_parameter_handles.w_p = param_find("FW_WR_P");
|
||||
_parameter_handles.w_i = param_find("FW_WR_I");
|
||||
_parameter_handles.w_ff = param_find("FW_WR_FF");
|
||||
_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
|
||||
_parameter_handles.w_rmax = param_find("FW_W_RMAX");
|
||||
|
||||
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
|
||||
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
|
||||
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
|
||||
|
||||
_parameter_handles.trim_roll = param_find("TRIM_ROLL");
|
||||
_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
|
||||
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
|
||||
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
|
||||
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
|
||||
|
||||
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
|
||||
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
|
||||
_parameter_handles.man_roll_scale = param_find("FW_MAN_R_SC");
|
||||
_parameter_handles.man_pitch_scale = param_find("FW_MAN_P_SC");
|
||||
_parameter_handles.man_yaw_scale = param_find("FW_MAN_Y_SC");
|
||||
|
||||
_parameter_handles.acro_max_x_rate = param_find("FW_ACRO_X_MAX");
|
||||
_parameter_handles.acro_max_y_rate = param_find("FW_ACRO_Y_MAX");
|
||||
_parameter_handles.acro_max_z_rate = param_find("FW_ACRO_Z_MAX");
|
||||
|
||||
_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
|
||||
_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
|
||||
|
||||
_parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
|
||||
|
||||
_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
|
||||
|
||||
// initialize to invalid VTOL type
|
||||
_parameters.vtol_type = -1;
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
FixedwingAttitudeControl::~FixedwingAttitudeControl()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
perf_free(_nonfinite_input_perf);
|
||||
perf_free(_nonfinite_output_perf);
|
||||
}
|
||||
|
||||
int
|
||||
FixedwingAttitudeControl::parameters_update()
|
||||
{
|
||||
|
||||
param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
|
||||
param_get(_parameter_handles.p_p, &(_parameters.p_p));
|
||||
param_get(_parameter_handles.p_i, &(_parameters.p_i));
|
||||
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
|
||||
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
|
||||
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
|
||||
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
|
||||
|
||||
param_get(_parameter_handles.r_tc, &(_parameters.r_tc));
|
||||
param_get(_parameter_handles.r_p, &(_parameters.r_p));
|
||||
param_get(_parameter_handles.r_i, &(_parameters.r_i));
|
||||
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
|
||||
|
||||
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
|
||||
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
|
||||
|
||||
param_get(_parameter_handles.y_p, &(_parameters.y_p));
|
||||
param_get(_parameter_handles.y_i, &(_parameters.y_i));
|
||||
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
|
||||
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
|
||||
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
|
||||
param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff));
|
||||
|
||||
int32_t wheel_enabled = 0;
|
||||
param_get(_parameter_handles.w_en, &wheel_enabled);
|
||||
_parameters.w_en = (wheel_enabled == 1);
|
||||
|
||||
param_get(_parameter_handles.w_p, &(_parameters.w_p));
|
||||
param_get(_parameter_handles.w_i, &(_parameters.w_i));
|
||||
param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
|
||||
param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
|
||||
param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
|
||||
|
||||
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
|
||||
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
|
||||
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
|
||||
|
||||
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
|
||||
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
|
||||
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
|
||||
param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
|
||||
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
|
||||
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
|
||||
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
|
||||
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
|
||||
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
|
||||
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
|
||||
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
|
||||
param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale));
|
||||
param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
|
||||
param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
|
||||
|
||||
param_get(_parameter_handles.acro_max_x_rate, &(_parameters.acro_max_x_rate_rad));
|
||||
param_get(_parameter_handles.acro_max_y_rate, &(_parameters.acro_max_y_rate_rad));
|
||||
param_get(_parameter_handles.acro_max_z_rate, &(_parameters.acro_max_z_rate_rad));
|
||||
_parameters.acro_max_x_rate_rad = math::radians(_parameters.acro_max_x_rate_rad);
|
||||
_parameters.acro_max_y_rate_rad = math::radians(_parameters.acro_max_y_rate_rad);
|
||||
_parameters.acro_max_z_rate_rad = math::radians(_parameters.acro_max_z_rate_rad);
|
||||
|
||||
param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
|
||||
param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
|
||||
|
||||
param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
|
||||
|
||||
if (_vehicle_status.is_vtol) {
|
||||
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
|
||||
}
|
||||
|
||||
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
|
||||
|
||||
/* pitch control parameters */
|
||||
_pitch_ctrl.set_time_constant(_parameters.p_tc);
|
||||
_pitch_ctrl.set_k_p(_parameters.p_p);
|
||||
_pitch_ctrl.set_k_i(_parameters.p_i);
|
||||
_pitch_ctrl.set_k_ff(_parameters.p_ff);
|
||||
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
|
||||
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
|
||||
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
|
||||
|
||||
/* roll control parameters */
|
||||
_roll_ctrl.set_time_constant(_parameters.r_tc);
|
||||
_roll_ctrl.set_k_p(_parameters.r_p);
|
||||
_roll_ctrl.set_k_i(_parameters.r_i);
|
||||
_roll_ctrl.set_k_ff(_parameters.r_ff);
|
||||
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
|
||||
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
|
||||
|
||||
/* yaw control parameters */
|
||||
_yaw_ctrl.set_k_p(_parameters.y_p);
|
||||
_yaw_ctrl.set_k_i(_parameters.y_i);
|
||||
_yaw_ctrl.set_k_ff(_parameters.y_ff);
|
||||
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
|
||||
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
|
||||
|
||||
/* wheel control parameters */
|
||||
_wheel_ctrl.set_k_p(_parameters.w_p);
|
||||
_wheel_ctrl.set_k_i(_parameters.w_i);
|
||||
_wheel_ctrl.set_k_ff(_parameters.w_ff);
|
||||
_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
|
||||
_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_control_mode_poll()
|
||||
{
|
||||
bool vcontrol_mode_updated;
|
||||
|
||||
/* Check if vehicle control mode has changed */
|
||||
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
|
||||
|
||||
if (vcontrol_mode_updated) {
|
||||
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_manual_poll()
|
||||
{
|
||||
bool manual_updated;
|
||||
|
||||
/* get pilots inputs */
|
||||
orb_check(_manual_sub, &manual_updated);
|
||||
|
||||
if (manual_updated) {
|
||||
|
||||
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_setpoint_poll()
|
||||
{
|
||||
/* check if there is a new setpoint */
|
||||
bool att_sp_updated;
|
||||
orb_check(_att_sp_sub, &att_sp_updated);
|
||||
|
||||
if (att_sp_updated) {
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::global_pos_poll()
|
||||
{
|
||||
/* check if there is a new global position */
|
||||
bool global_pos_updated;
|
||||
orb_check(_global_pos_sub, &global_pos_updated);
|
||||
|
||||
if (global_pos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
bool vehicle_status_updated;
|
||||
orb_check(_vehicle_status_sub, &vehicle_status_updated);
|
||||
|
||||
if (vehicle_status_updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
|
||||
|
||||
/* set correct uORB ID, depending on if vehicle is VTOL or not */
|
||||
if (!_rates_sp_id) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
|
||||
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
|
||||
|
||||
_parameter_handles.vtol_type = param_find("VT_TYPE");
|
||||
|
||||
parameters_update();
|
||||
|
||||
} else {
|
||||
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_land_detected_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
bool vehicle_land_detected_updated;
|
||||
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
|
||||
|
||||
if (vehicle_land_detected_updated) {
|
||||
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::battery_status_poll()
|
||||
{
|
||||
/* check if there is a new message */
|
||||
bool updated;
|
||||
orb_check(_battery_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
|
||||
}
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::run()
|
||||
{
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
|
||||
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
|
||||
parameters_update();
|
||||
|
||||
/* get an initial update for all sensor and status data */
|
||||
vehicle_setpoint_poll();
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_manual_poll();
|
||||
vehicle_status_poll();
|
||||
vehicle_land_detected_poll();
|
||||
battery_status_poll();
|
||||
|
||||
/* wakeup source */
|
||||
px4_pollfd_struct_t fds[1];
|
||||
|
||||
/* Setup of loop */
|
||||
fds[0].fd = _att_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
while (!should_exit()) {
|
||||
static int loop_counter = 0;
|
||||
|
||||
/* wait for up to 500ms for data */
|
||||
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
|
||||
|
||||
/* timed out - periodic check for _task_should_exit, etc. */
|
||||
if (pret == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||
if (pret < 0) {
|
||||
PX4_WARN("poll error %d, %d", pret, errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* only update parameters if they changed */
|
||||
bool params_updated = false;
|
||||
orb_check(_params_sub, ¶ms_updated);
|
||||
|
||||
if (params_updated) {
|
||||
/* read from param to clear updated flag */
|
||||
parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
static uint64_t last_run = 0;
|
||||
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
/* guard against too large deltaT's */
|
||||
if (deltaT > 1.0f) {
|
||||
deltaT = 0.01f;
|
||||
}
|
||||
|
||||
/* load local copies */
|
||||
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
|
||||
|
||||
/* get current rotation matrix and euler angles from control state quaternions */
|
||||
math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]);
|
||||
_R = q_att.to_dcm();
|
||||
|
||||
math::Vector<3> euler_angles;
|
||||
euler_angles = _R.to_euler();
|
||||
_roll = euler_angles(0);
|
||||
_pitch = euler_angles(1);
|
||||
_yaw = euler_angles(2);
|
||||
|
||||
if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {
|
||||
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
|
||||
*
|
||||
* Since the VTOL airframe is initialized as a multicopter we need to
|
||||
* modify the estimated attitude for the fixed wing operation.
|
||||
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
|
||||
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
|
||||
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
|
||||
* Additionally, in order to get the correct sign of the pitch, we need to multiply
|
||||
* the new x axis of the rotation matrix with -1
|
||||
*
|
||||
* original: modified:
|
||||
*
|
||||
* Rxx Ryx Rzx -Rzx Ryx Rxx
|
||||
* Rxy Ryy Rzy -Rzy Ryy Rxy
|
||||
* Rxz Ryz Rzz -Rzz Ryz Rxz
|
||||
* */
|
||||
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
|
||||
|
||||
/* move z to x */
|
||||
R_adapted(0, 0) = _R(0, 2);
|
||||
R_adapted(1, 0) = _R(1, 2);
|
||||
R_adapted(2, 0) = _R(2, 2);
|
||||
|
||||
/* move x to z */
|
||||
R_adapted(0, 2) = _R(0, 0);
|
||||
R_adapted(1, 2) = _R(1, 0);
|
||||
R_adapted(2, 2) = _R(2, 0);
|
||||
|
||||
/* change direction of pitch (convert to right handed system) */
|
||||
R_adapted(0, 0) = -R_adapted(0, 0);
|
||||
R_adapted(1, 0) = -R_adapted(1, 0);
|
||||
R_adapted(2, 0) = -R_adapted(2, 0);
|
||||
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
|
||||
|
||||
/* fill in new attitude data */
|
||||
_R = R_adapted;
|
||||
_roll = euler_angles(0);
|
||||
_pitch = euler_angles(1);
|
||||
_yaw = euler_angles(2);
|
||||
|
||||
/* lastly, roll- and yawspeed have to be swaped */
|
||||
float helper = _att.rollspeed;
|
||||
_att.rollspeed = -_att.yawspeed;
|
||||
_att.yawspeed = helper;
|
||||
}
|
||||
|
||||
updateSubscriptions();
|
||||
vehicle_setpoint_poll();
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_manual_poll();
|
||||
global_pos_poll();
|
||||
vehicle_status_poll();
|
||||
vehicle_land_detected_poll();
|
||||
battery_status_poll();
|
||||
|
||||
// the position controller will not emit attitude setpoints in some modes
|
||||
// we need to make sure that this flag is reset
|
||||
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
|
||||
|
||||
/* lock integrator until control is started */
|
||||
bool lock_integrator = !(_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing);
|
||||
|
||||
/* Simple handling of failsafe: deploy parachute if failsafe is on */
|
||||
if (_vcontrol_mode.flag_control_termination_enabled) {
|
||||
_actuators_airframe.control[7] = 1.0f;
|
||||
//warnx("_actuators_airframe.control[1] = 1.0f;");
|
||||
|
||||
} else {
|
||||
_actuators_airframe.control[7] = 0.0f;
|
||||
//warnx("_actuators_airframe.control[1] = -1.0f;");
|
||||
}
|
||||
|
||||
/* if we are in rotary wing mode, do nothing */
|
||||
if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* default flaps to center */
|
||||
float flap_control = 0.0f;
|
||||
|
||||
/* map flaps by default to manual if valid */
|
||||
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
|
||||
&& fabsf(_parameters.flaps_scale) > 0.01f) {
|
||||
flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled
|
||||
&& fabsf(_parameters.flaps_scale) > 0.01f) {
|
||||
flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
|
||||
}
|
||||
|
||||
// move the actual control value continuous with time, full flap travel in 1sec
|
||||
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
|
||||
_flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT;
|
||||
|
||||
} else {
|
||||
_flaps_applied = flap_control;
|
||||
}
|
||||
|
||||
/* default flaperon to center */
|
||||
float flaperon_control = 0.0f;
|
||||
|
||||
/* map flaperons by default to manual if valid */
|
||||
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
|
||||
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
|
||||
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled
|
||||
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
|
||||
flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
|
||||
}
|
||||
|
||||
// move the actual control value continuous with time, full flap travel in 1sec
|
||||
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
|
||||
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT;
|
||||
|
||||
} else {
|
||||
_flaperons_applied = flaperon_control;
|
||||
}
|
||||
|
||||
// Check if we are in rattitude mode and the pilot is above the threshold on pitch
|
||||
if (_vcontrol_mode.flag_control_rattitude_enabled) {
|
||||
if (fabsf(_manual.y) > _parameters.rattitude_thres ||
|
||||
fabsf(_manual.x) > _parameters.rattitude_thres) {
|
||||
_vcontrol_mode.flag_control_attitude_enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
/* decide if in stabilized or full manual control */
|
||||
if (_vcontrol_mode.flag_control_rates_enabled) {
|
||||
/* scale around tuning airspeed */
|
||||
float airspeed;
|
||||
|
||||
/* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */
|
||||
const bool airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s)
|
||||
&& (hrt_elapsed_time(&_sub_airspeed.get().timestamp) < 1e6);
|
||||
|
||||
if (airspeed_valid) {
|
||||
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
|
||||
airspeed = math::max(0.5f, _sub_airspeed.get().indicated_airspeed_m_s);
|
||||
|
||||
} else {
|
||||
airspeed = _parameters.airspeed_trim;
|
||||
perf_count(_nonfinite_input_perf);
|
||||
}
|
||||
|
||||
/*
|
||||
* For scaling our actuators using anything less than the min (close to stall)
|
||||
* speed doesn't make any sense - its the strongest reasonable deflection we
|
||||
* want to do in flight and its the baseline a human pilot would choose.
|
||||
*
|
||||
* Forcing the scaling to this value allows reasonable handheld tests.
|
||||
*/
|
||||
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
|
||||
airspeed);
|
||||
|
||||
/* Use min airspeed to calculate ground speed scaling region.
|
||||
* Don't scale below gspd_scaling_trim
|
||||
*/
|
||||
float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
|
||||
_global_pos.vel_e * _global_pos.vel_e);
|
||||
float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f);
|
||||
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
|
||||
|
||||
// in STABILIZED mode we need to generate the attitude setpoint
|
||||
// from manual user inputs
|
||||
if (!_vcontrol_mode.flag_control_climb_rate_enabled && !_vcontrol_mode.flag_control_offboard_enabled) {
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad;
|
||||
_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max);
|
||||
_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
|
||||
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
|
||||
_att_sp.yaw_body = 0.0f;
|
||||
_att_sp.thrust = _manual.z;
|
||||
|
||||
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
|
||||
q.copyTo(_att_sp.q_d);
|
||||
_att_sp.q_d_valid = true;
|
||||
|
||||
int instance;
|
||||
orb_publish_auto(_attitude_setpoint_id, &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
/* reset integrals where needed */
|
||||
if (_att_sp.roll_reset_integral) {
|
||||
_roll_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
if (_att_sp.pitch_reset_integral) {
|
||||
_pitch_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
if (_att_sp.yaw_reset_integral) {
|
||||
_yaw_ctrl.reset_integrator();
|
||||
_wheel_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
/* Reset integrators if the aircraft is on ground
|
||||
* or a multicopter (but not transitioning VTOL)
|
||||
*/
|
||||
if (_vehicle_land_detected.landed
|
||||
|| (_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode)) {
|
||||
|
||||
_roll_ctrl.reset_integrator();
|
||||
_pitch_ctrl.reset_integrator();
|
||||
_yaw_ctrl.reset_integrator();
|
||||
_wheel_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
float roll_sp = _att_sp.roll_body;
|
||||
float pitch_sp = _att_sp.pitch_body;
|
||||
float yaw_sp = _att_sp.yaw_body;
|
||||
float throttle_sp = _att_sp.thrust;
|
||||
|
||||
/* Prepare data for attitude controllers */
|
||||
struct ECL_ControlData control_input = {};
|
||||
control_input.roll = _roll;
|
||||
control_input.pitch = _pitch;
|
||||
control_input.yaw = _yaw;
|
||||
control_input.body_x_rate = _att.rollspeed;
|
||||
control_input.body_y_rate = _att.pitchspeed;
|
||||
control_input.body_z_rate = _att.yawspeed;
|
||||
control_input.roll_setpoint = roll_sp;
|
||||
control_input.pitch_setpoint = pitch_sp;
|
||||
control_input.yaw_setpoint = yaw_sp;
|
||||
control_input.airspeed_min = _parameters.airspeed_min;
|
||||
control_input.airspeed_max = _parameters.airspeed_max;
|
||||
control_input.airspeed = airspeed;
|
||||
control_input.scaler = airspeed_scaling;
|
||||
control_input.lock_integrator = lock_integrator;
|
||||
control_input.groundspeed = groundspeed;
|
||||
control_input.groundspeed_scaler = groundspeed_scaler;
|
||||
|
||||
/* Run attitude controllers */
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
|
||||
_roll_ctrl.control_attitude(control_input);
|
||||
_pitch_ctrl.control_attitude(control_input);
|
||||
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
|
||||
_wheel_ctrl.control_attitude(control_input);
|
||||
|
||||
/* Update input data for rate controllers */
|
||||
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
|
||||
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
|
||||
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
|
||||
|
||||
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
|
||||
float roll_u = _roll_ctrl.control_euler_rate(control_input);
|
||||
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
|
||||
_parameters.trim_roll;
|
||||
|
||||
if (!PX4_ISFINITE(roll_u)) {
|
||||
_roll_ctrl.reset_integrator();
|
||||
perf_count(_nonfinite_output_perf);
|
||||
}
|
||||
|
||||
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
|
||||
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
|
||||
_parameters.trim_pitch;
|
||||
|
||||
if (!PX4_ISFINITE(pitch_u)) {
|
||||
_pitch_ctrl.reset_integrator();
|
||||
perf_count(_nonfinite_output_perf);
|
||||
}
|
||||
|
||||
float yaw_u = 0.0f;
|
||||
|
||||
if (_parameters.w_en && _att_sp.fw_control_yaw) {
|
||||
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
|
||||
|
||||
} else {
|
||||
yaw_u = _yaw_ctrl.control_euler_rate(control_input);
|
||||
}
|
||||
|
||||
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
|
||||
_parameters.trim_yaw;
|
||||
|
||||
/* add in manual rudder control in manual modes */
|
||||
if (_vcontrol_mode.flag_control_manual_enabled) {
|
||||
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(yaw_u)) {
|
||||
_yaw_ctrl.reset_integrator();
|
||||
_wheel_ctrl.reset_integrator();
|
||||
perf_count(_nonfinite_output_perf);
|
||||
}
|
||||
|
||||
/* throttle passed through if it is finite and if no engine failure was detected */
|
||||
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp)
|
||||
&& !_vehicle_status.engine_failure) ? throttle_sp : 0.0f;
|
||||
|
||||
/* scale effort by battery status */
|
||||
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
|
||||
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
|
||||
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
|
||||
}
|
||||
|
||||
} else {
|
||||
perf_count(_nonfinite_input_perf);
|
||||
}
|
||||
|
||||
} else {
|
||||
// pure rate control
|
||||
_roll_ctrl.set_bodyrate_setpoint(_manual.y * _parameters.acro_max_x_rate_rad);
|
||||
_pitch_ctrl.set_bodyrate_setpoint(-_manual.x * _parameters.acro_max_y_rate_rad);
|
||||
_yaw_ctrl.set_bodyrate_setpoint(_manual.r * _parameters.acro_max_z_rate_rad);
|
||||
|
||||
float roll_u = _roll_ctrl.control_bodyrate(control_input);
|
||||
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
|
||||
_parameters.trim_roll;
|
||||
|
||||
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
|
||||
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
|
||||
_parameters.trim_pitch;
|
||||
|
||||
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
|
||||
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
|
||||
_parameters.trim_yaw;
|
||||
|
||||
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
|
||||
}
|
||||
|
||||
/*
|
||||
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
|
||||
* only once available
|
||||
*/
|
||||
_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
|
||||
_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
|
||||
_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
|
||||
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_rate_sp_pub != nullptr) {
|
||||
/* publish the attitude rates setpoint */
|
||||
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
|
||||
|
||||
} else if (_rates_sp_id) {
|
||||
/* advertise the attitude rates setpoint */
|
||||
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
|
||||
}
|
||||
|
||||
rate_ctrl_status_s rate_ctrl_status;
|
||||
rate_ctrl_status.timestamp = hrt_absolute_time();
|
||||
rate_ctrl_status.rollspeed = _att.rollspeed;
|
||||
rate_ctrl_status.pitchspeed = _att.pitchspeed;
|
||||
rate_ctrl_status.yawspeed = _att.yawspeed;
|
||||
rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
|
||||
rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
|
||||
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
|
||||
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
|
||||
|
||||
int instance;
|
||||
orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT);
|
||||
|
||||
} else {
|
||||
/* manual/direct control */
|
||||
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll;
|
||||
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale +
|
||||
_parameters.trim_pitch;
|
||||
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
|
||||
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
|
||||
}
|
||||
|
||||
// Add feed-forward from roll control output to yaw control output
|
||||
// This can be used to counteract the adverse yaw effect when rolling the plane
|
||||
_actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain(
|
||||
_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
|
||||
|
||||
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
|
||||
_actuators.control[5] = _manual.aux1;
|
||||
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
|
||||
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
|
||||
_actuators.control[7] = _manual.aux3;
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
_actuators.timestamp_sample = _att.timestamp;
|
||||
_actuators_airframe.timestamp = hrt_absolute_time();
|
||||
_actuators_airframe.timestamp_sample = _att.timestamp;
|
||||
|
||||
/* Only publish if any of the proper modes are enabled */
|
||||
if (_vcontrol_mode.flag_control_rates_enabled ||
|
||||
_vcontrol_mode.flag_control_attitude_enabled ||
|
||||
_vcontrol_mode.flag_control_manual_enabled) {
|
||||
/* publish the actuator controls */
|
||||
if (_actuators_0_pub != nullptr) {
|
||||
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
|
||||
|
||||
} else if (_actuators_id) {
|
||||
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
|
||||
}
|
||||
|
||||
if (_actuators_2_pub != nullptr) {
|
||||
/* publish the actuator controls*/
|
||||
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
loop_counter++;
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
}
|
||||
|
||||
FixedwingAttitudeControl *FixedwingAttitudeControl::instantiate(int argc, char *argv[])
|
||||
{
|
||||
return new FixedwingAttitudeControl();
|
||||
}
|
||||
|
||||
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("fw_att_controol",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_ATTITUDE_CONTROL,
|
||||
1500,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int FixedwingAttitudeControl::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int FixedwingAttitudeControl::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
fw_att_control is the fixed wing attitude controller.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("fw_att_control", "controller");
|
||||
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int FixedwingAttitudeControl::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
|
||||
// perf?
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int fw_att_control_main(int argc, char *argv[])
|
||||
{
|
||||
return FixedwingAttitudeControl::main(argc, argv);
|
||||
}
|
||||
Reference in New Issue
Block a user