mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-04 07:40:34 +08:00
added VTOL attitude control module
This commit is contained in:
Roman Bapst
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013, 2014 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# VTOL attitude controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = vtol_att_control
|
||||
|
||||
SRCS = vtol_att_control_main.cpp
|
||||
@@ -0,0 +1,716 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file VTOL_att_control_main.cpp
|
||||
* Implementation of an attitude controller for VTOL airframes. This module receives data
|
||||
* from both the fixed wing- and the multicopter attitude controllers and processes it.
|
||||
* It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
|
||||
* flight or transition). It also publishes the resulting controls on the actuator controls topics.
|
||||
*
|
||||
* @author Roman Bapst <bapstr@ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/circuit_breaker.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include "drivers/drv_pwm_output.h"
|
||||
#include <nuttx/fs/nxffs.h>
|
||||
#include <nuttx/fs/ioctl.h>
|
||||
|
||||
#include <nuttx/mtd.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
|
||||
|
||||
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
|
||||
|
||||
class VtolAttitudeControl
|
||||
{
|
||||
public:
|
||||
|
||||
VtolAttitudeControl();
|
||||
~VtolAttitudeControl();
|
||||
|
||||
int start(); /* start the task and return OK on success */
|
||||
|
||||
|
||||
private:
|
||||
//******************flags & handlers******************************************************
|
||||
bool _task_should_exit;
|
||||
int _control_task; //task handle for VTOL attitude controller
|
||||
|
||||
/* handlers for subscriptions */
|
||||
int _v_att_sub; //vehicle attitude subscription
|
||||
int _v_att_sp_sub; //vehicle attitude setpoint subscription
|
||||
int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
|
||||
int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
|
||||
int _v_control_mode_sub; //vehicle control mode subscription
|
||||
int _params_sub; //parameter updates subscription
|
||||
int _manual_control_sp_sub; //manual control setpoint subscription
|
||||
int _armed_sub; //arming status subscription
|
||||
|
||||
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
|
||||
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
|
||||
|
||||
//handlers for publishers
|
||||
orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust)
|
||||
orb_advert_t _actuators_1_pub;
|
||||
orb_advert_t _vtol_vehicle_status_pub;
|
||||
orb_advert_t _v_rates_sp_pub;
|
||||
//*******************data containers***********************************************************
|
||||
struct vehicle_attitude_s _v_att; //vehicle attitude
|
||||
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
|
||||
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
|
||||
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
|
||||
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
|
||||
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
|
||||
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
|
||||
struct vtol_vehicle_status_s _vtol_vehicle_status;
|
||||
struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer
|
||||
struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
|
||||
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
|
||||
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
|
||||
struct actuator_armed_s _armed; //actuator arming status
|
||||
|
||||
struct {
|
||||
float min_pwm_mc; //pwm value for idle in mc mode
|
||||
} _params;
|
||||
|
||||
struct {
|
||||
param_t min_pwm_mc;
|
||||
} _params_handles;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
/* for multicopters it is usual to have a non-zero idle speed of the engines
|
||||
* for fixed wings we want to have an idle speed of zero since we do not want
|
||||
* to waste energy when gliding. */
|
||||
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
|
||||
|
||||
//*****************Member functions***********************************************************************
|
||||
|
||||
void task_main(); //main task
|
||||
static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
|
||||
|
||||
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
|
||||
void vehicle_manual_poll(); //Check for changes in manual inputs.
|
||||
void arming_status_poll(); //Check for arming status updates.
|
||||
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
|
||||
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
|
||||
void vehicle_rates_sp_mc_poll();
|
||||
void vehicle_rates_sp_fw_poll();
|
||||
void parameters_update_poll(); //Check if parameters have changed
|
||||
int parameters_update(); //Update local paraemter cache
|
||||
void fill_mc_att_control_output(); //write mc_att_control results to actuator message
|
||||
void fill_fw_att_control_output(); //write fw_att_control results to actuator message
|
||||
void fill_mc_att_rates_sp();
|
||||
void fill_fw_att_rates_sp();
|
||||
void set_idle_fw();
|
||||
void set_idle_mc();
|
||||
};
|
||||
|
||||
namespace VTOL_att_control
|
||||
{
|
||||
VtolAttitudeControl *g_control;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
VtolAttitudeControl::VtolAttitudeControl() :
|
||||
_task_should_exit(false),
|
||||
_control_task(-1),
|
||||
|
||||
//init subscription handlers
|
||||
_v_att_sub(-1),
|
||||
_v_att_sp_sub(-1),
|
||||
_mc_virtual_v_rates_sp_sub(-1),
|
||||
_fw_virtual_v_rates_sp_sub(-1),
|
||||
_v_control_mode_sub(-1),
|
||||
_params_sub(-1),
|
||||
_manual_control_sp_sub(-1),
|
||||
_armed_sub(-1),
|
||||
|
||||
//init publication handlers
|
||||
_actuators_0_pub(-1),
|
||||
_actuators_1_pub(-1),
|
||||
_vtol_vehicle_status_pub(-1),
|
||||
_v_rates_sp_pub(-1),
|
||||
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
|
||||
{
|
||||
|
||||
flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */
|
||||
|
||||
memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
|
||||
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
|
||||
memset(&_v_att, 0, sizeof(_v_att));
|
||||
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
|
||||
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
|
||||
memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
|
||||
memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
|
||||
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
|
||||
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
|
||||
memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
|
||||
memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
|
||||
memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
|
||||
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
|
||||
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
|
||||
memset(&_armed, 0, sizeof(_armed));
|
||||
|
||||
_params_handles.min_pwm_mc = param_find("PWM_MIN");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
VtolAttitudeControl::~VtolAttitudeControl()
|
||||
{
|
||||
if (_control_task != -1) {
|
||||
/* task wakes up every 100ms or so at the longest */
|
||||
_task_should_exit = true;
|
||||
|
||||
/* wait for a second for the task to quit at our request */
|
||||
unsigned i = 0;
|
||||
|
||||
do {
|
||||
/* wait 20ms */
|
||||
usleep(20000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (++i > 50) {
|
||||
task_delete(_control_task);
|
||||
break;
|
||||
}
|
||||
} while (_control_task != -1);
|
||||
}
|
||||
|
||||
VTOL_att_control::g_control = nullptr;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for changes in vehicle control mode.
|
||||
*/
|
||||
void VtolAttitudeControl::vehicle_control_mode_poll()
|
||||
{
|
||||
bool updated;
|
||||
|
||||
/* Check if vehicle control mode has changed */
|
||||
orb_check(_v_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for changes in manual inputs.
|
||||
*/
|
||||
void VtolAttitudeControl::vehicle_manual_poll()
|
||||
{
|
||||
bool updated;
|
||||
|
||||
/* get pilots inputs */
|
||||
orb_check(_manual_control_sp_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
|
||||
}
|
||||
}
|
||||
/**
|
||||
* Check for arming status updates.
|
||||
*/
|
||||
void VtolAttitudeControl::arming_status_poll()
|
||||
{
|
||||
/* check if there is a new setpoint */
|
||||
bool updated;
|
||||
orb_check(_armed_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for inputs from mc attitude controller.
|
||||
*/
|
||||
void VtolAttitudeControl::actuator_controls_mc_poll()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_actuator_inputs_mc, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for inputs from fw attitude controller.
|
||||
*/
|
||||
void VtolAttitudeControl::actuator_controls_fw_poll()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_actuator_inputs_fw, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for attitude rates setpoint from mc attitude controller
|
||||
*/
|
||||
void VtolAttitudeControl::vehicle_rates_sp_mc_poll()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_mc_virtual_v_rates_sp_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub , &_mc_virtual_v_rates_sp);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for attitude rates setpoint from fw attitude controller
|
||||
*/
|
||||
void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_fw_virtual_v_rates_sp_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub , &_fw_virtual_v_rates_sp);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for parameter updates.
|
||||
*/
|
||||
void
|
||||
VtolAttitudeControl::parameters_update_poll()
|
||||
{
|
||||
bool updated;
|
||||
|
||||
/* Check if parameters have changed */
|
||||
orb_check(_params_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
struct parameter_update_s param_update;
|
||||
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
|
||||
parameters_update();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update parameters.
|
||||
*/
|
||||
int
|
||||
VtolAttitudeControl::parameters_update()
|
||||
{
|
||||
/* idle pwm */
|
||||
float v;
|
||||
param_get(_params_handles.min_pwm_mc, &v);
|
||||
_params.min_pwm_mc = v;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* Prepare message to acutators with data from mc attitude controller.
|
||||
*/
|
||||
void VtolAttitudeControl::fill_mc_att_control_output()
|
||||
{
|
||||
_actuators_out_0.control[0] = _actuators_mc_in.control[0];
|
||||
_actuators_out_0.control[1] = _actuators_mc_in.control[1];
|
||||
_actuators_out_0.control[2] = _actuators_mc_in.control[2];
|
||||
_actuators_out_0.control[3] = _actuators_mc_in.control[3];
|
||||
//set neutral position for elevons
|
||||
_actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon
|
||||
_actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon
|
||||
}
|
||||
|
||||
/**
|
||||
* Prepare message to acutators with data from fw attitude controller.
|
||||
*/
|
||||
void VtolAttitudeControl::fill_fw_att_control_output()
|
||||
{
|
||||
/*For the first test in fw mode, only use engines for thrust!!!*/
|
||||
_actuators_out_0.control[0] = 0;
|
||||
_actuators_out_0.control[1] = 0;
|
||||
_actuators_out_0.control[2] = 0;
|
||||
_actuators_out_0.control[3] = _actuators_fw_in.control[3];
|
||||
/*controls for the elevons */
|
||||
_actuators_out_1.control[0] = -_actuators_fw_in.control[0]; /*roll elevon*/
|
||||
_actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */
|
||||
}
|
||||
|
||||
/**
|
||||
* Prepare message for mc attitude rates setpoint topic
|
||||
*/
|
||||
void VtolAttitudeControl::fill_mc_att_rates_sp()
|
||||
{
|
||||
_v_rates_sp.roll = _mc_virtual_v_rates_sp.roll;
|
||||
_v_rates_sp.pitch = _mc_virtual_v_rates_sp.pitch;
|
||||
_v_rates_sp.yaw = _mc_virtual_v_rates_sp.yaw;
|
||||
_v_rates_sp.thrust = _mc_virtual_v_rates_sp.thrust;
|
||||
}
|
||||
|
||||
/**
|
||||
* Prepare message for fw attitude rates setpoint topic
|
||||
*/
|
||||
void VtolAttitudeControl::fill_fw_att_rates_sp()
|
||||
{
|
||||
_v_rates_sp.roll = _fw_virtual_v_rates_sp.roll;
|
||||
_v_rates_sp.pitch = _fw_virtual_v_rates_sp.pitch;
|
||||
_v_rates_sp.yaw = _fw_virtual_v_rates_sp.yaw;
|
||||
_v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust;
|
||||
}
|
||||
|
||||
/**
|
||||
* Adjust idle speed for fw mode.
|
||||
*/
|
||||
void VtolAttitudeControl::set_idle_fw()
|
||||
{
|
||||
int ret;
|
||||
char *dev = PWM_OUTPUT_DEVICE_PATH;
|
||||
int fd = open(dev, 0);
|
||||
|
||||
if (fd < 0) {err(1, "can't open %s", dev);}
|
||||
|
||||
unsigned pwm_value = PWM_LOWEST_MIN;
|
||||
struct pwm_output_values pwm_values;
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
|
||||
pwm_values.values[i] = pwm_value;
|
||||
pwm_values.channel_count++;
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
|
||||
|
||||
if (ret != OK) {errx(ret, "failed setting min values");}
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adjust idle speed for mc mode.
|
||||
*/
|
||||
void VtolAttitudeControl::set_idle_mc()
|
||||
{
|
||||
int ret;
|
||||
unsigned servo_count;
|
||||
char *dev = PWM_OUTPUT_DEVICE_PATH;
|
||||
int fd = open(dev, 0);
|
||||
|
||||
if (fd < 0) {err(1, "can't open %s", dev);}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
|
||||
unsigned pwm_value = 1100;
|
||||
struct pwm_output_values pwm_values;
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
pwm_values.values[i] = pwm_value;
|
||||
pwm_values.channel_count++;
|
||||
}
|
||||
|
||||
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
|
||||
|
||||
if (ret != OK) {errx(ret, "failed setting min values");}
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
void
|
||||
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
VTOL_att_control::g_control->task_main();
|
||||
}
|
||||
|
||||
void VtolAttitudeControl::task_main()
|
||||
{
|
||||
warnx("started");
|
||||
fflush(stdout);
|
||||
|
||||
/* do subscriptions */
|
||||
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
|
||||
_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
|
||||
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
|
||||
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
|
||||
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
|
||||
|
||||
parameters_update(); /*initialize parameter cache/*
|
||||
|
||||
/* wakeup source*/
|
||||
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
|
||||
|
||||
fds[0].fd = _actuator_inputs_mc;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = _actuator_inputs_fw;
|
||||
fds[1].events = POLLIN;
|
||||
fds[2].fd = _params_sub;
|
||||
fds[2].events = POLLIN;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
/*Advertise/Publish vtol vehicle status*/
|
||||
if (_vtol_vehicle_status_pub > 0) {
|
||||
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
|
||||
|
||||
} else {
|
||||
_vtol_vehicle_status.timestamp = hrt_absolute_time();
|
||||
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
|
||||
}
|
||||
|
||||
/* wait for up to 100ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
|
||||
|
||||
|
||||
/* timed out - periodic check for _task_should_exit */
|
||||
if (pret == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||
if (pret < 0) {
|
||||
warn("poll error %d, %d", pret, errno);
|
||||
/* sleep a bit before next try */
|
||||
usleep(100000);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
|
||||
vehicle_manual_poll(); //Check for changes in manual inputs.
|
||||
arming_status_poll(); //Check for arming status updates.
|
||||
actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
|
||||
actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
|
||||
vehicle_rates_sp_mc_poll();
|
||||
vehicle_rates_sp_fw_poll();
|
||||
parameters_update_poll();
|
||||
|
||||
if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */
|
||||
_vtol_vehicle_status.vtol_in_rw_mode = true;
|
||||
|
||||
if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */
|
||||
set_idle_mc();
|
||||
flag_idle_mc = true;
|
||||
}
|
||||
|
||||
/* got data from mc_att_controller */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
vehicle_manual_poll(); /* update remote input */
|
||||
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
|
||||
|
||||
fill_mc_att_control_output();
|
||||
fill_mc_att_rates_sp();
|
||||
|
||||
if (_actuators_0_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
|
||||
|
||||
} else {
|
||||
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
|
||||
}
|
||||
|
||||
if (_actuators_1_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
|
||||
|
||||
} else {
|
||||
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */
|
||||
_vtol_vehicle_status.vtol_in_rw_mode = false;
|
||||
|
||||
if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */
|
||||
set_idle_fw();
|
||||
flag_idle_mc = false;
|
||||
}
|
||||
|
||||
if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */
|
||||
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
|
||||
vehicle_manual_poll(); //update remote input
|
||||
|
||||
fill_fw_att_control_output();
|
||||
fill_fw_att_rates_sp();
|
||||
|
||||
if (_actuators_0_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
|
||||
|
||||
} else {
|
||||
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
|
||||
}
|
||||
|
||||
if (_actuators_1_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
|
||||
|
||||
} else {
|
||||
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// publish the attitude rates setpoint
|
||||
if(_v_rates_sp_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp);
|
||||
}
|
||||
else {
|
||||
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp);
|
||||
}
|
||||
}
|
||||
|
||||
warnx("exit");
|
||||
_control_task = -1;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
int
|
||||
VtolAttitudeControl::start()
|
||||
{
|
||||
ASSERT(_control_task == -1);
|
||||
|
||||
/* start the task */
|
||||
_control_task = task_spawn_cmd("vtol_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
2048,
|
||||
(main_t)&VtolAttitudeControl::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_control_task < 0) {
|
||||
warn("task start failed");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
int vtol_att_control_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1) {
|
||||
errx(1, "usage: vtol_att_control {start|stop|status}");
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (VTOL_att_control::g_control != nullptr) {
|
||||
errx(1, "already running");
|
||||
}
|
||||
|
||||
VTOL_att_control::g_control = new VtolAttitudeControl;
|
||||
|
||||
if (VTOL_att_control::g_control == nullptr) {
|
||||
errx(1, "alloc failed");
|
||||
}
|
||||
|
||||
if (OK != VTOL_att_control::g_control->start()) {
|
||||
delete VTOL_att_control::g_control;
|
||||
VTOL_att_control::g_control = nullptr;
|
||||
err(1, "start failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (VTOL_att_control::g_control == nullptr) {
|
||||
errx(1, "not running");
|
||||
}
|
||||
|
||||
delete VTOL_att_control::g_control;
|
||||
VTOL_att_control::g_control = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (VTOL_att_control::g_control) {
|
||||
errx(0, "running");
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
|
||||
warnx("unrecognized command");
|
||||
return 1;
|
||||
}
|
||||
Reference in New Issue
Block a user