forked from Archive/PX4-Autopilot
Merge pull request #1481 from PX4/vtol_merge
VTOL architecture working with caipirinha
This commit is contained in:
commit
a2bcbabd16
|
@ -0,0 +1,16 @@
|
|||
#!nsh
|
||||
#
|
||||
# Generic configuration file for caipirinha VTOL version
|
||||
#
|
||||
# Roman Bapst <bapstr@ethz.ch>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.vtol_defaults
|
||||
|
||||
set MIXER FMU_caipirinha_vtol
|
||||
|
||||
set PWM_OUTPUTS 12
|
||||
set PWM_MAX 2000
|
||||
set PWM_RATE 400
|
||||
param set VTOL_MOT_COUNT 2
|
||||
param set IDLE_PWM_MC 1080
|
|
@ -15,6 +15,7 @@
|
|||
# 10000 .. 10999 Wide arm / H frame
|
||||
# 11000 .. 11999 Hexa Cox
|
||||
# 12000 .. 12999 Octo Cox
|
||||
# 13000 .. 13999 VTOL
|
||||
|
||||
#
|
||||
# Simulation
|
||||
|
@ -237,3 +238,13 @@ if param compare SYS_AUTOSTART 12001
|
|||
then
|
||||
sh /etc/init.d/12001_octo_cox
|
||||
fi
|
||||
|
||||
# 13000 is historically reserved for the quadshot
|
||||
|
||||
#
|
||||
# VTOL caipririnha
|
||||
#
|
||||
if param compare SYS_AUTOSTART 13001
|
||||
then
|
||||
sh /etc/init.d/13001_caipirinha_vtol
|
||||
fi
|
||||
|
|
|
@ -0,0 +1,15 @@
|
|||
#!nsh
|
||||
#
|
||||
# Standard apps for vtol:
|
||||
# att & pos estimator, att & pos control.
|
||||
#
|
||||
|
||||
attitude_estimator_ekf start
|
||||
#ekf_att_pos_estimator start
|
||||
position_estimator_inav start
|
||||
|
||||
vtol_att_control start
|
||||
mc_att_control start
|
||||
mc_pos_control start
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
|
@ -0,0 +1,39 @@
|
|||
#!nsh
|
||||
|
||||
set VEHICLE_TYPE vtol
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
#
|
||||
#Default controller parameters for MC
|
||||
#
|
||||
param set MC_ROLL_P 6.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
param set MC_ROLLRATE_I 0.0
|
||||
param set MC_ROLLRATE_D 0.003
|
||||
param set MC_PITCH_P 6.0
|
||||
param set MC_PITCHRATE_P 0.2
|
||||
param set MC_PITCHRATE_I 0.0
|
||||
param set MC_PITCHRATE_D 0.003
|
||||
param set MC_YAW_P 4
|
||||
param set MC_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAW_FF 0.3
|
||||
|
||||
#
|
||||
# Default parameters for FW
|
||||
#
|
||||
param set FW_PR_FF 0.3
|
||||
param set FW_PR_I 0.000
|
||||
param set FW_PR_IMAX 0.2
|
||||
param set FW_PR_P 0.02
|
||||
param set FW_RR_FF 0.3
|
||||
param set FW_RR_I 0.00
|
||||
param set FW_RR_IMAX 0.2
|
||||
param set FW_RR_P 0.02
|
||||
fi
|
||||
|
||||
set PWM_DISARMED 900
|
||||
set PWM_MIN 1000
|
||||
set PWM_MAX 2000
|
|
@ -551,6 +551,45 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# VTOL setup
|
||||
#
|
||||
if [ $VEHICLE_TYPE == vtol ]
|
||||
then
|
||||
echo "[init] Vehicle type: VTOL"
|
||||
|
||||
if [ $MIXER == none ]
|
||||
then
|
||||
echo "Default mixer for vtol not defined"
|
||||
fi
|
||||
|
||||
if [ $MAV_TYPE == none ]
|
||||
then
|
||||
# Use mixer to detect vehicle type
|
||||
if [ $MIXER == FMU_caipirinha_vtol ]
|
||||
then
|
||||
set MAV_TYPE 19
|
||||
fi
|
||||
fi
|
||||
|
||||
# Still no MAV_TYPE found
|
||||
if [ $MAV_TYPE == none ]
|
||||
then
|
||||
echo "Unknown MAV_TYPE"
|
||||
else
|
||||
param set MAV_TYPE $MAV_TYPE
|
||||
fi
|
||||
|
||||
# Load mixer and configure outputs
|
||||
sh /etc/init.d/rc.interface
|
||||
|
||||
# Start standard vtol apps
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
then
|
||||
sh /etc/init.d/rc.vtol_apps
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the navigator
|
||||
#
|
||||
|
|
|
@ -0,0 +1,16 @@
|
|||
#!nsh
|
||||
# Caipirinha vtol mixer for PX4FMU
|
||||
#
|
||||
#===========================
|
||||
R: 2- 10000 10000 10000 0
|
||||
|
||||
#mixer for the elevons
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 0 10000 10000 0 -10000 10000
|
||||
S: 1 1 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 0 10000 10000 0 -10000 10000
|
||||
S: 1 1 -10000 -10000 0 -10000 10000
|
|
@ -86,6 +86,7 @@ MODULES += modules/fw_pos_control_l1
|
|||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
MODULES += modules/vtol_att_control
|
||||
|
||||
#
|
||||
# Logging
|
||||
|
|
|
@ -571,6 +571,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
|||
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
|
||||
att.R_valid = true;
|
||||
|
||||
// compute secondary attitude
|
||||
math::Matrix<3, 3> R_adapted; //modified rotation matrix
|
||||
R_adapted = R;
|
||||
|
||||
//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);
|
||||
math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation
|
||||
euler_angles_sec = R_adapted.to_euler();
|
||||
|
||||
att.roll_sec = euler_angles_sec(0);
|
||||
att.pitch_sec = euler_angles_sec(1);
|
||||
att.yaw_sec = euler_angles_sec(2);
|
||||
|
||||
memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec));
|
||||
|
||||
att.rollspeed_sec = -x_aposteriori[2];
|
||||
att.pitchspeed_sec = x_aposteriori[1];
|
||||
att.yawspeed_sec = x_aposteriori[0];
|
||||
att.rollacc_sec = -x_aposteriori[5];
|
||||
att.pitchacc_sec = x_aposteriori[4];
|
||||
att.yawacc_sec = x_aposteriori[3];
|
||||
|
||||
att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
|
||||
att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
|
||||
att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
|
||||
|
||||
|
||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
|
|
@ -82,6 +82,7 @@
|
|||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
#include <drivers/drv_led.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
@ -149,6 +150,9 @@ enum MAV_MODE_FLAG {
|
|||
/* Mavlink file descriptors */
|
||||
static int mavlink_fd = 0;
|
||||
|
||||
/* Syste autostart ID */
|
||||
static int autostart_id;
|
||||
|
||||
/* flags */
|
||||
static bool commander_initialized = false;
|
||||
static volatile bool thread_should_exit = false; /**< daemon exit flag */
|
||||
|
@ -732,6 +736,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
|
||||
param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
|
||||
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
|
||||
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
|
||||
|
||||
/* welcome user */
|
||||
warnx("starting");
|
||||
|
@ -1014,6 +1019,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
struct actuator_controls_s actuator_controls;
|
||||
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
||||
|
||||
/* Subscribe to vtol vehicle status topic */
|
||||
int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
|
||||
struct vtol_vehicle_status_s vtol_status;
|
||||
memset(&vtol_status, 0, sizeof(vtol_status));
|
||||
vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing
|
||||
|
||||
|
||||
control_status_leds(&status, &armed, true);
|
||||
|
||||
/* now initialized */
|
||||
|
@ -1070,7 +1082,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status.system_type == VEHICLE_TYPE_TRICOPTER ||
|
||||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
|
||||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
|
||||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
|
||||
status.system_type == VEHICLE_TYPE_OCTOROTOR ||
|
||||
(status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
|
||||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
|
||||
|
||||
status.is_rotary_wing = true;
|
||||
|
||||
} else {
|
||||
|
@ -1106,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
|
||||
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
|
||||
param_get(_param_ef_time_thres, &ef_time_thres);
|
||||
param_get(_param_autostart_id, &autostart_id);
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &updated);
|
||||
|
@ -1231,6 +1247,19 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
/* update vtol vehicle status*/
|
||||
orb_check(vtol_vehicle_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
/* vtol status changed */
|
||||
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
|
||||
|
||||
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
|
||||
if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
|
||||
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
|
||||
}
|
||||
}
|
||||
|
||||
/* update global position estimate */
|
||||
orb_check(global_position_sub, &updated);
|
||||
|
||||
|
@ -2189,7 +2218,13 @@ set_control_mode()
|
|||
/* set vehicle_control_mode according to set_navigation_state */
|
||||
control_mode.flag_armed = armed.armed;
|
||||
/* TODO: check this */
|
||||
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
|
||||
if (autostart_id < 13000 || autostart_id >= 14000) {
|
||||
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
|
||||
|
||||
} else {
|
||||
control_mode.flag_external_manual_override_ok = false;
|
||||
}
|
||||
|
||||
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
|
||||
control_mode.flag_control_offboard_enabled = false;
|
||||
|
||||
|
|
|
@ -35,8 +35,9 @@
|
|||
* @file fw_att_control_main.c
|
||||
* Implementation of a generic attitude controller based on classic orthogonal PIDs.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Roman Bapst <bapstr@ethz.ch>
|
||||
*
|
||||
*/
|
||||
|
||||
|
@ -92,12 +93,12 @@ public:
|
|||
FixedwingAttitudeControl();
|
||||
|
||||
/**
|
||||
* Destructor, also kills the sensors task.
|
||||
* Destructor, also kills the main task.
|
||||
*/
|
||||
~FixedwingAttitudeControl();
|
||||
|
||||
/**
|
||||
* Start the sensors task.
|
||||
* Start the main task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
|
@ -112,9 +113,9 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
bool _task_should_exit; /**< if true, attitude control task should exit */
|
||||
bool _task_running; /**< if true, task is running in its mainloop */
|
||||
int _control_task; /**< task handle for sensor task */
|
||||
int _control_task; /**< task handle */
|
||||
|
||||
int _att_sub; /**< vehicle attitude subscription */
|
||||
int _accel_sub; /**< accelerometer subscription */
|
||||
|
@ -130,11 +131,15 @@ private:
|
|||
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
|
||||
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
|
||||
orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
|
||||
orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */
|
||||
|
||||
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
|
||||
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
|
||||
|
||||
struct vehicle_attitude_s _att; /**< vehicle attitude */
|
||||
struct accel_report _accel; /**< body frame accelerations */
|
||||
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
|
||||
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
|
||||
struct manual_control_setpoint_s _manual; /**< r/c channel data */
|
||||
struct airspeed_s _airspeed; /**< airspeed */
|
||||
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
|
||||
|
@ -189,6 +194,8 @@ private:
|
|||
float man_roll_max; /**< Max Roll in rad */
|
||||
float man_pitch_max; /**< Max Pitch in rad */
|
||||
|
||||
param_t autostart_id; /* indicates which airframe is used */
|
||||
|
||||
} _parameters; /**< local copies of interesting parameters */
|
||||
|
||||
struct {
|
||||
|
@ -228,6 +235,8 @@ private:
|
|||
param_t pitchsp_offset_deg;
|
||||
param_t man_roll_max;
|
||||
param_t man_pitch_max;
|
||||
|
||||
param_t autostart_id; /* indicates which airframe is used */
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
|
@ -289,7 +298,7 @@ private:
|
|||
static void task_main_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Main sensor collection task.
|
||||
* Main attitude controller collection task.
|
||||
*/
|
||||
void task_main();
|
||||
|
||||
|
@ -327,7 +336,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||
_rate_sp_pub(-1),
|
||||
_attitude_sp_pub(-1),
|
||||
_actuators_0_pub(-1),
|
||||
_actuators_1_pub(-1),
|
||||
_actuators_2_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
|
||||
|
@ -341,6 +350,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||
_att = {};
|
||||
_accel = {};
|
||||
_att_sp = {};
|
||||
_rates_sp = {};
|
||||
_manual = {};
|
||||
_airspeed = {};
|
||||
_vcontrol_mode = {};
|
||||
|
@ -386,8 +396,19 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
|
||||
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
|
||||
|
||||
_parameter_handles.autostart_id = param_find("SYS_AUTOSTART");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
// set correct uORB ID, depending on if vehicle is VTOL or not
|
||||
if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
|
||||
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
|
||||
}
|
||||
else {
|
||||
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
}
|
||||
}
|
||||
|
||||
FixedwingAttitudeControl::~FixedwingAttitudeControl()
|
||||
|
@ -462,6 +483,7 @@ FixedwingAttitudeControl::parameters_update()
|
|||
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
|
||||
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
|
||||
|
||||
param_get(_parameter_handles.autostart_id, &_parameters.autostart_id);
|
||||
|
||||
/* pitch control parameters */
|
||||
_pitch_ctrl.set_time_constant(_parameters.tconst);
|
||||
|
@ -497,7 +519,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
|
|||
{
|
||||
bool vcontrol_mode_updated;
|
||||
|
||||
/* Check HIL state if vehicle status has changed */
|
||||
/* Check if vehicle control mode has changed */
|
||||
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
|
||||
|
||||
if (vcontrol_mode_updated) {
|
||||
|
@ -529,7 +551,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
|
|||
|
||||
if (airspeed_updated) {
|
||||
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
|
||||
// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -679,6 +700,65 @@ FixedwingAttitudeControl::task_main()
|
|||
/* load local copies */
|
||||
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
|
||||
|
||||
if (_parameters.autostart_id >= 13000
|
||||
&& _parameters.autostart_id <= 13999) { //vehicle type is VTOL, need to modify attitude!
|
||||
/* The following modification to the attitude is vehicle specific and in this case applies
|
||||
to tail-sitter models !!!
|
||||
|
||||
* 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; //original rotation matrix
|
||||
math::Matrix<3, 3> R_adapted; //modified rotation matrix
|
||||
R.set(_att.R);
|
||||
R_adapted.set(_att.R);
|
||||
|
||||
//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);
|
||||
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
|
||||
euler_angles = R_adapted.to_euler();
|
||||
//fill in new attitude data
|
||||
_att.roll = euler_angles(0);
|
||||
_att.pitch = euler_angles(1);
|
||||
_att.yaw = euler_angles(2);
|
||||
_att.R[0][0] = R_adapted(0, 0);
|
||||
_att.R[0][1] = R_adapted(0, 1);
|
||||
_att.R[0][2] = R_adapted(0, 2);
|
||||
_att.R[1][0] = R_adapted(1, 0);
|
||||
_att.R[1][1] = R_adapted(1, 1);
|
||||
_att.R[1][2] = R_adapted(1, 2);
|
||||
_att.R[2][0] = R_adapted(2, 0);
|
||||
_att.R[2][1] = R_adapted(2, 1);
|
||||
_att.R[2][2] = R_adapted(2, 2);
|
||||
|
||||
// lastly, roll- and yawspeed have to be swaped
|
||||
float helper = _att.rollspeed;
|
||||
_att.rollspeed = -_att.yawspeed;
|
||||
_att.yawspeed = helper;
|
||||
}
|
||||
|
||||
vehicle_airspeed_poll();
|
||||
|
||||
vehicle_setpoint_poll();
|
||||
|
@ -696,7 +776,7 @@ FixedwingAttitudeControl::task_main()
|
|||
/* lock integrator until control is started */
|
||||
bool lock_integrator;
|
||||
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
|
||||
lock_integrator = false;
|
||||
|
||||
} else {
|
||||
|
@ -705,10 +785,10 @@ FixedwingAttitudeControl::task_main()
|
|||
|
||||
/* Simple handling of failsafe: deploy parachute if failsafe is on */
|
||||
if (_vcontrol_mode.flag_control_termination_enabled) {
|
||||
_actuators_airframe.control[1] = 1.0f;
|
||||
_actuators_airframe.control[7] = 1.0f;
|
||||
// warnx("_actuators_airframe.control[1] = 1.0f;");
|
||||
} else {
|
||||
_actuators_airframe.control[1] = 0.0f;
|
||||
_actuators_airframe.control[7] = 0.0f;
|
||||
// warnx("_actuators_airframe.control[1] = -1.0f;");
|
||||
}
|
||||
|
||||
|
@ -820,18 +900,18 @@ FixedwingAttitudeControl::task_main()
|
|||
att_sp.thrust = throttle_sp;
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
if (_attitude_sp_pub > 0) {
|
||||
if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
|
||||
|
||||
} else {
|
||||
} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
|
||||
/* advertise and publish */
|
||||
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
}
|
||||
}
|
||||
|
||||
/* If the aircraft is on ground reset the integrators */
|
||||
if (_vehicle_status.condition_landed) {
|
||||
if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
|
||||
_roll_ctrl.reset_integrator();
|
||||
_pitch_ctrl.reset_integrator();
|
||||
_yaw_ctrl.reset_integrator();
|
||||
|
@ -933,20 +1013,18 @@ FixedwingAttitudeControl::task_main()
|
|||
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
|
||||
* only once available
|
||||
*/
|
||||
vehicle_rates_setpoint_s rates_sp;
|
||||
rates_sp.roll = _roll_ctrl.get_desired_rate();
|
||||
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
|
||||
rates_sp.yaw = _yaw_ctrl.get_desired_rate();
|
||||
_rates_sp.roll = _roll_ctrl.get_desired_rate();
|
||||
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
|
||||
_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
|
||||
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_rate_sp_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
|
||||
|
||||
/* publish the attitude rates setpoint */
|
||||
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
/* advertise the attitude rates setpoint */
|
||||
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
@ -966,28 +1044,21 @@ FixedwingAttitudeControl::task_main()
|
|||
_actuators.timestamp = hrt_absolute_time();
|
||||
_actuators_airframe.timestamp = hrt_absolute_time();
|
||||
|
||||
/* publish the actuator controls */
|
||||
if (_actuators_0_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
|
||||
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
|
||||
} else {
|
||||
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
|
||||
}
|
||||
|
||||
if (_actuators_2_pub > 0) {
|
||||
/* publish the actuator controls*/
|
||||
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
|
||||
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
|
||||
}
|
||||
|
||||
if (_actuators_1_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
|
||||
// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
|
||||
// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
|
||||
// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
|
||||
// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
loop_counter++;
|
||||
|
|
|
@ -1351,7 +1351,10 @@ protected:
|
|||
/* scale outputs depending on system type */
|
||||
if (system_type == MAV_TYPE_QUADROTOR ||
|
||||
system_type == MAV_TYPE_HEXAROTOR ||
|
||||
system_type == MAV_TYPE_OCTOROTOR) {
|
||||
system_type == MAV_TYPE_OCTOROTOR ||
|
||||
system_type == MAV_TYPE_VTOL_DUOROTOR ||
|
||||
system_type == MAV_TYPE_VTOL_QUADROTOR) {
|
||||
|
||||
/* multirotors: set number of rotor outputs depending on type */
|
||||
|
||||
unsigned n;
|
||||
|
@ -1365,6 +1368,14 @@ protected:
|
|||
n = 6;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_VTOL_DUOROTOR:
|
||||
n = 2;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_VTOL_QUADROTOR:
|
||||
n = 4;
|
||||
break;
|
||||
|
||||
default:
|
||||
n = 8;
|
||||
break;
|
||||
|
|
|
@ -66,6 +66,7 @@
|
|||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
@ -96,12 +97,12 @@ public:
|
|||
MulticopterAttitudeControl();
|
||||
|
||||
/**
|
||||
* Destructor, also kills the sensors task.
|
||||
* Destructor, also kills the main task
|
||||
*/
|
||||
~MulticopterAttitudeControl();
|
||||
|
||||
/**
|
||||
* Start the sensors task.
|
||||
* Start the multicopter attitude control task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
|
@ -109,8 +110,8 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
int _control_task; /**< task handle for sensor task */
|
||||
bool _task_should_exit; /**< if true, task_main() should exit */
|
||||
int _control_task; /**< task handle */
|
||||
|
||||
int _v_att_sub; /**< vehicle attitude subscription */
|
||||
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
|
||||
|
@ -119,11 +120,15 @@ private:
|
|||
int _params_sub; /**< parameter updates subscription */
|
||||
int _manual_control_sp_sub; /**< manual control setpoint subscription */
|
||||
int _armed_sub; /**< arming status subscription */
|
||||
int _vehicle_status_sub; /**< vehicle status subscription */
|
||||
|
||||
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
|
||||
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
|
||||
|
||||
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
|
||||
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
|
||||
|
||||
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
|
||||
|
||||
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
|
||||
|
@ -133,6 +138,7 @@ private:
|
|||
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
|
||||
struct actuator_controls_s _actuators; /**< actuator controls */
|
||||
struct actuator_armed_s _armed; /**< actuator arming status */
|
||||
struct vehicle_status_s _vehicle_status; /**< vehicle status */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
|
@ -168,6 +174,8 @@ private:
|
|||
param_t acro_roll_max;
|
||||
param_t acro_pitch_max;
|
||||
param_t acro_yaw_max;
|
||||
|
||||
param_t autostart_id; //what frame are we using?
|
||||
} _params_handles; /**< handles for interesting parameters */
|
||||
|
||||
struct {
|
||||
|
@ -182,6 +190,8 @@ private:
|
|||
float man_pitch_max;
|
||||
float man_yaw_max;
|
||||
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
|
||||
|
||||
param_t autostart_id;
|
||||
} _params;
|
||||
|
||||
/**
|
||||
|
@ -229,6 +239,11 @@ private:
|
|||
*/
|
||||
void control_attitude_rates(float dt);
|
||||
|
||||
/**
|
||||
* Check for vehicle status updates.
|
||||
*/
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
|
@ -264,6 +279,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_params_sub(-1),
|
||||
_manual_control_sp_sub(-1),
|
||||
_armed_sub(-1),
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_att_sp_pub(-1),
|
||||
|
@ -283,6 +299,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
|
||||
memset(&_actuators, 0, sizeof(_actuators));
|
||||
memset(&_armed, 0, sizeof(_armed));
|
||||
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
|
||||
_vehicle_status.is_rotary_wing = true;
|
||||
|
||||
_params.att_p.zero();
|
||||
_params.rate_p.zero();
|
||||
|
@ -295,6 +313,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_params.man_yaw_max = 0.0f;
|
||||
_params.acro_rate_max.zero();
|
||||
|
||||
_params.autostart_id = 0; //default
|
||||
|
||||
_rates_prev.zero();
|
||||
_rates_sp.zero();
|
||||
_rates_int.zero();
|
||||
|
@ -324,8 +344,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
|
||||
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
|
||||
|
||||
_params_handles.autostart_id = param_find("SYS_AUTOSTART");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
// set correct uORB ID, depending on if vehicle is VTOL or not
|
||||
if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
|
||||
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
|
||||
}
|
||||
else {
|
||||
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
}
|
||||
}
|
||||
|
||||
MulticopterAttitudeControl::~MulticopterAttitudeControl()
|
||||
|
@ -409,6 +440,8 @@ MulticopterAttitudeControl::parameters_update()
|
|||
|
||||
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
|
||||
|
||||
param_get(_params_handles.autostart_id, &_params.autostart_id);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
@ -417,7 +450,7 @@ MulticopterAttitudeControl::parameter_update_poll()
|
|||
{
|
||||
bool updated;
|
||||
|
||||
/* Check HIL state if vehicle status has changed */
|
||||
/* Check if parameters have changed */
|
||||
orb_check(_params_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
|
@ -432,7 +465,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
|
|||
{
|
||||
bool updated;
|
||||
|
||||
/* Check HIL state if vehicle status has changed */
|
||||
/* Check if vehicle control mode has changed */
|
||||
orb_check(_v_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
|
@ -489,6 +522,18 @@ MulticopterAttitudeControl::arming_status_poll()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::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);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Attitude controller.
|
||||
* Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
|
||||
|
@ -585,7 +630,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
|||
}
|
||||
|
||||
/* publish the attitude setpoint if needed */
|
||||
if (publish_att_sp) {
|
||||
if (publish_att_sp && _vehicle_status.is_rotary_wing) {
|
||||
_v_att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_att_sp_pub > 0) {
|
||||
|
@ -682,7 +727,7 @@ void
|
|||
MulticopterAttitudeControl::control_attitude_rates(float dt)
|
||||
{
|
||||
/* reset integral if disarmed */
|
||||
if (!_armed.armed) {
|
||||
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
|
||||
_rates_int.zero();
|
||||
}
|
||||
|
||||
|
@ -732,6 +777,7 @@ MulticopterAttitudeControl::task_main()
|
|||
_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));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* initialize parameters cache */
|
||||
parameters_update();
|
||||
|
@ -783,6 +829,7 @@ MulticopterAttitudeControl::task_main()
|
|||
vehicle_control_mode_poll();
|
||||
arming_status_poll();
|
||||
vehicle_manual_poll();
|
||||
vehicle_status_poll();
|
||||
|
||||
if (_v_control_mode.flag_control_attitude_enabled) {
|
||||
control_attitude(dt);
|
||||
|
@ -795,10 +842,10 @@ MulticopterAttitudeControl::task_main()
|
|||
_v_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_v_rates_sp_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
|
||||
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
|
||||
|
||||
} else {
|
||||
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
|
||||
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
@ -819,10 +866,10 @@ MulticopterAttitudeControl::task_main()
|
|||
_v_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_v_rates_sp_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
|
||||
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
|
||||
|
||||
} else {
|
||||
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
|
||||
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
@ -847,11 +894,12 @@ MulticopterAttitudeControl::task_main()
|
|||
|
||||
if (!_actuators_0_circuit_breaker_enabled) {
|
||||
if (_actuators_0_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
|
||||
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
|
||||
|
||||
} else {
|
||||
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
|
||||
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -941,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
struct actuator_outputs_s act_outputs;
|
||||
struct actuator_controls_s act_controls;
|
||||
struct actuator_controls_s act_controls1;
|
||||
struct vehicle_local_position_s local_pos;
|
||||
struct vehicle_local_position_setpoint_s local_pos_sp;
|
||||
struct vehicle_global_position_s global_pos;
|
||||
|
@ -1022,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
int rates_sp_sub;
|
||||
int act_outputs_sub;
|
||||
int act_controls_sub;
|
||||
int act_controls_1_sub;
|
||||
int local_pos_sub;
|
||||
int local_pos_sp_sub;
|
||||
int global_pos_sub;
|
||||
|
@ -1055,6 +1057,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
|
||||
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
|
||||
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
|
||||
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
|
@ -1375,6 +1378,18 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
|
||||
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
|
||||
LOGBUFFER_WRITE_AND_COUNT(ATT);
|
||||
// secondary attitude
|
||||
log_msg.msg_type = LOG_ATT2_MSG;
|
||||
log_msg.body.log_ATT.roll = buf.att.roll_sec;
|
||||
log_msg.body.log_ATT.pitch = buf.att.pitch_sec;
|
||||
log_msg.body.log_ATT.yaw = buf.att.yaw_sec;
|
||||
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec;
|
||||
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec;
|
||||
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec;
|
||||
log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0];
|
||||
log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1];
|
||||
log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2];
|
||||
LOGBUFFER_WRITE_AND_COUNT(ATT);
|
||||
}
|
||||
|
||||
/* --- ATTITUDE SETPOINT --- */
|
||||
|
@ -1413,6 +1428,16 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
LOGBUFFER_WRITE_AND_COUNT(ATTC);
|
||||
}
|
||||
|
||||
/* --- ACTUATOR CONTROL FW VTOL --- */
|
||||
if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
|
||||
log_msg.msg_type = LOG_ATC1_MSG;
|
||||
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
|
||||
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
|
||||
log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
|
||||
log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
|
||||
LOGBUFFER_WRITE_AND_COUNT(ATTC);
|
||||
}
|
||||
|
||||
/* --- LOCAL POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
|
||||
log_msg.msg_type = LOG_LPOS_MSG;
|
||||
|
|
|
@ -50,6 +50,7 @@
|
|||
#pragma pack(push, 1)
|
||||
/* --- ATT - ATTITUDE --- */
|
||||
#define LOG_ATT_MSG 2
|
||||
#define LOG_ATT2_MSG 41
|
||||
struct log_ATT_s {
|
||||
float roll;
|
||||
float pitch;
|
||||
|
@ -149,6 +150,7 @@ struct log_GPS_s {
|
|||
|
||||
/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
|
||||
#define LOG_ATTC_MSG 9
|
||||
#define LOG_ATC1_MSG 40
|
||||
struct log_ATTC_s {
|
||||
float roll;
|
||||
float pitch;
|
||||
|
@ -422,7 +424,6 @@ struct log_ENCD_s {
|
|||
float vel1;
|
||||
};
|
||||
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
|
@ -450,7 +451,8 @@ struct log_PARM_s {
|
|||
/* construct list of all message formats */
|
||||
static const struct log_format_s log_formats[] = {
|
||||
/* business-level messages, ID < 0x80 */
|
||||
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
|
||||
LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
|
||||
LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
|
||||
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
|
||||
LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
|
@ -459,7 +461,8 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
|
||||
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
|
||||
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
|
||||
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
|
||||
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
|
||||
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
|
||||
LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
|
||||
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
|
||||
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
|
||||
|
|
|
@ -466,6 +466,7 @@ public:
|
|||
OCTA_X,
|
||||
OCTA_PLUS,
|
||||
OCTA_COX,
|
||||
TWIN_ENGINE, /**< VTOL: one engine on each wing */
|
||||
|
||||
MAX_GEOMETRY
|
||||
};
|
||||
|
|
|
@ -76,6 +76,7 @@ float constrain(float val, float min, float max)
|
|||
/*
|
||||
* These tables automatically generated by multi_tables - do not edit.
|
||||
*/
|
||||
|
||||
const MultirotorMixer::Rotor _config_quad_x[] = {
|
||||
{ -0.707107, 0.707107, 1.00 },
|
||||
{ 0.707107, -0.707107, 1.00 },
|
||||
|
@ -88,11 +89,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
|
|||
{ 0.000000, 1.000000, -1.00 },
|
||||
{ -0.000000, -1.000000, -1.00 },
|
||||
};
|
||||
//Add table for quad in V configuration, which is not generated by multi_tables!
|
||||
const MultirotorMixer::Rotor _config_quad_v[] = {
|
||||
{ -0.927184, 0.374607, 1.00 },
|
||||
{ 0.694658, -0.719340, 1.00 },
|
||||
{ 0.927184, 0.374607, -1.00 },
|
||||
{ -0.694658, -0.719340, -1.00 },
|
||||
{ -0.3223, 0.9466, 0.4242 },
|
||||
{ 0.3223, -0.9466, 1.0000 },
|
||||
{ 0.3223, 0.9466, -0.4242 },
|
||||
{ -0.3223, -0.9466, -1.0000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_quad_wide[] = {
|
||||
{ -0.927184, 0.374607, 1.00 },
|
||||
|
@ -154,6 +156,11 @@ const MultirotorMixer::Rotor _config_octa_cox[] = {
|
|||
{ -0.707107, -0.707107, 1.00 },
|
||||
{ 0.707107, -0.707107, -1.00 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_duorotor[] = {
|
||||
{ -1.000000, 0.000000, 0.00 },
|
||||
{ 1.000000, 0.000000, 0.00 },
|
||||
};
|
||||
|
||||
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
|
||||
&_config_quad_x[0],
|
||||
&_config_quad_plus[0],
|
||||
|
@ -165,6 +172,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
|
|||
&_config_octa_x[0],
|
||||
&_config_octa_plus[0],
|
||||
&_config_octa_cox[0],
|
||||
&_config_duorotor[0],
|
||||
};
|
||||
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
|
||||
4, /* quad_x */
|
||||
|
@ -177,6 +185,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
|
|||
8, /* octa_x */
|
||||
8, /* octa_plus */
|
||||
8, /* octa_cox */
|
||||
2, /* twin_engine */
|
||||
};
|
||||
|
||||
}
|
||||
|
@ -274,6 +283,8 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
|||
} else if (!strcmp(geomname, "8c")) {
|
||||
geometry = MultirotorMixer::OCTA_COX;
|
||||
|
||||
} else if (!strcmp(geomname, "2-")) {
|
||||
geometry = MultirotorMixer::TWIN_ENGINE;
|
||||
} else {
|
||||
debug("unrecognised geometry '%s'", geomname);
|
||||
return nullptr;
|
||||
|
|
|
@ -6,6 +6,7 @@
|
|||
proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
|
||||
proc rcos {a} { expr cos([rad $a])}
|
||||
|
||||
|
||||
set quad_x {
|
||||
45 CCW
|
||||
-135 CCW
|
||||
|
@ -20,12 +21,6 @@ set quad_plus {
|
|||
180 CW
|
||||
}
|
||||
|
||||
set quad_v {
|
||||
68 CCW
|
||||
-136 CCW
|
||||
-68 CW
|
||||
136 CW
|
||||
}
|
||||
|
||||
set quad_wide {
|
||||
68 CCW
|
||||
|
@ -94,7 +89,9 @@ set octa_cox {
|
|||
-135 CW
|
||||
}
|
||||
|
||||
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
|
||||
|
||||
set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
|
||||
|
||||
|
||||
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
|
||||
|
||||
|
|
|
@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s);
|
|||
#include "topics/vehicle_status.h"
|
||||
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
|
||||
|
||||
#include "topics/vtol_vehicle_status.h"
|
||||
ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s);
|
||||
|
||||
#include "topics/safety.h"
|
||||
ORB_DEFINE(safety, struct safety_s);
|
||||
|
||||
|
@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
|
|||
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
|
||||
ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
|
||||
ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
|
||||
|
||||
#include "topics/rc_channels.h"
|
||||
ORB_DEFINE(rc_channels, struct rc_channels_s);
|
||||
|
@ -148,6 +153,8 @@ ORB_DEFINE(fence, unsigned);
|
|||
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
|
||||
ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
|
||||
ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
|
||||
|
||||
#include "topics/manual_control_setpoint.h"
|
||||
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
|
||||
|
@ -182,6 +189,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
|
|||
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
|
||||
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
|
||||
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
|
||||
//Virtual control groups, used for VTOL operation
|
||||
ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
|
||||
ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
|
||||
|
||||
#include "topics/actuator_armed.h"
|
||||
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
|
||||
|
|
|
@ -74,5 +74,8 @@ ORB_DECLARE(actuator_controls_0);
|
|||
ORB_DECLARE(actuator_controls_1);
|
||||
ORB_DECLARE(actuator_controls_2);
|
||||
ORB_DECLARE(actuator_controls_3);
|
||||
ORB_DECLARE(actuator_controls_virtual_mc);
|
||||
ORB_DECLARE(actuator_controls_virtual_fw);
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
@ -80,6 +80,23 @@ struct vehicle_attitude_s {
|
|||
bool R_valid; /**< Rotation matrix valid */
|
||||
bool q_valid; /**< Quaternion valid */
|
||||
|
||||
// secondary attitude, use for VTOL
|
||||
float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */
|
||||
float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */
|
||||
float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */
|
||||
float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */
|
||||
float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */
|
||||
float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */
|
||||
float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */
|
||||
float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */
|
||||
float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */
|
||||
float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */
|
||||
float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
|
||||
float q_sec[4]; /**< Quaternion (NED) */
|
||||
float g_comp_sec[3]; /**< Compensated gravity vector */
|
||||
bool R_valid_sec; /**< Rotation matrix valid */
|
||||
bool q_valid_sec; /**< Quaternion valid */
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s {
|
|||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vehicle_attitude_setpoint);
|
||||
ORB_DECLARE(mc_virtual_attitude_setpoint);
|
||||
ORB_DECLARE(fw_virtual_attitude_setpoint);
|
||||
|
||||
#endif /* TOPIC_ARDRONE_CONTROL_H_ */
|
||||
|
|
|
@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s {
|
|||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vehicle_rates_setpoint);
|
||||
|
||||
ORB_DECLARE(mc_virtual_rates_setpoint);
|
||||
ORB_DECLARE(fw_virtual_rates_setpoint);
|
||||
#endif
|
||||
|
|
|
@ -147,7 +147,10 @@ enum VEHICLE_TYPE {
|
|||
VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
|
||||
VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
|
||||
VEHICLE_TYPE_KITE = 17, /* Kite | */
|
||||
VEHICLE_TYPE_ENUM_END = 18, /* | */
|
||||
VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
|
||||
VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
|
||||
VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
|
||||
VEHICLE_TYPE_ENUM_END = 21 /* | */
|
||||
};
|
||||
|
||||
enum VEHICLE_BATTERY_WARNING {
|
||||
|
|
|
@ -0,0 +1,66 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 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_status.h
|
||||
*
|
||||
* Vtol status topic
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_VTOL_STATUS_H
|
||||
#define TOPIC_VTOL_STATUS_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Indicates in which mode the vtol aircraft is in */
|
||||
struct vtol_vehicle_status_s {
|
||||
|
||||
uint64_t timestamp; /**< Microseconds since system boot */
|
||||
bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vtol_vehicle_status);
|
||||
|
||||
#endif
|
|
@ -0,0 +1,41 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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 \
|
||||
vtol_att_control_params.c
|
|
@ -0,0 +1,812 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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/airspeed.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 _airspeed_sub; // airspeed 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 airspeed_s _airspeed; // airspeed
|
||||
|
||||
struct {
|
||||
param_t idle_pwm_mc; //pwm value for idle in mc mode
|
||||
param_t vtol_motor_count;
|
||||
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
|
||||
float mc_airspeed_trim; // trim airspeed in multicopter mode
|
||||
float mc_airspeed_max; // max airpseed in multicopter mode
|
||||
} _params;
|
||||
|
||||
struct {
|
||||
param_t idle_pwm_mc;
|
||||
param_t vtol_motor_count;
|
||||
param_t mc_airspeed_min;
|
||||
param_t mc_airspeed_trim;
|
||||
param_t mc_airspeed_max;
|
||||
} _params_handles;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
|
||||
|
||||
/* 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"
|
||||
unsigned _motor_count; // number of motors
|
||||
|
||||
//*****************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 vehicle_airspeed_poll(); // Check for changes in airspeed
|
||||
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();
|
||||
void scale_mc_output();
|
||||
};
|
||||
|
||||
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),
|
||||
_airspeed_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, "vtol_att_control")),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
|
||||
{
|
||||
|
||||
flag_idle_mc = true;
|
||||
|
||||
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));
|
||||
memset(&_airspeed,0,sizeof(_airspeed));
|
||||
|
||||
_params.idle_pwm_mc = PWM_LOWEST_MIN;
|
||||
_params.vtol_motor_count = 0;
|
||||
|
||||
_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
|
||||
_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
|
||||
_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
|
||||
_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
|
||||
_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
|
||||
|
||||
/* 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 airspeed updates.
|
||||
*/
|
||||
void
|
||||
VtolAttitudeControl::vehicle_airspeed_poll() {
|
||||
bool updated;
|
||||
orb_check(_airspeed_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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()
|
||||
{
|
||||
float v;
|
||||
/* idle pwm for mc mode */
|
||||
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
|
||||
|
||||
/* vtol motor count */
|
||||
param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
|
||||
|
||||
/* vtol mc mode min airspeed */
|
||||
param_get(_params_handles.mc_airspeed_min, &v);
|
||||
_params.mc_airspeed_min = v;
|
||||
|
||||
/* vtol mc mode max airspeed */
|
||||
param_get(_params_handles.mc_airspeed_max, &v);
|
||||
_params.mc_airspeed_max = v;
|
||||
|
||||
/* vtol mc mode trim airspeed */
|
||||
param_get(_params_handles.mc_airspeed_trim, &v);
|
||||
_params.mc_airspeed_trim = 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
|
||||
// unused now but still logged
|
||||
_actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw
|
||||
_actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 < _params.vtol_motor_count; 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 = _params.idle_pwm_mc;
|
||||
struct pwm_output_values pwm_values;
|
||||
memset(&pwm_values, 0, sizeof(pwm_values));
|
||||
|
||||
for (unsigned i = 0; i < _params.vtol_motor_count; 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::scale_mc_output() {
|
||||
// scale around tuning airspeed
|
||||
float airspeed;
|
||||
|
||||
// if airspeed is not updating, we assume the normal average speed
|
||||
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
|
||||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
|
||||
airspeed = _params.mc_airspeed_trim;
|
||||
if (nonfinite) {
|
||||
perf_count(_nonfinite_input_perf);
|
||||
}
|
||||
} else {
|
||||
// prevent numerical drama by requiring 0.5 m/s minimal speed
|
||||
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
|
||||
}
|
||||
|
||||
// Sanity check if airspeed is consistent with throttle
|
||||
if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param
|
||||
airspeed = _params.mc_airspeed_trim;
|
||||
}
|
||||
|
||||
/*
|
||||
* 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 = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed);
|
||||
_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
|
||||
}
|
||||
|
||||
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));
|
||||
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
|
||||
_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
|
||||
|
||||
// make sure we start with idle in mc mode
|
||||
set_idle_mc();
|
||||
flag_idle_mc = true;
|
||||
|
||||
/* 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();
|
||||
vehicle_airspeed_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);
|
||||
// scale pitch control with airspeed
|
||||
scale_mc_output();
|
||||
|
||||
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;
|
||||
}
|
|
@ -0,0 +1,13 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
// number of engines
|
||||
PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
|
||||
// idle pwm in multicopter mode
|
||||
PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
|
||||
// min airspeed in multicopter mode
|
||||
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
|
||||
// max airspeed in multicopter mode
|
||||
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
|
||||
// trim airspeed in multicopter mode
|
||||
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
|
||||
|
Loading…
Reference in New Issue