Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #1481 from PX4/vtol_merge

Browse Source 
VTOL architecture working with caipirinha
This commit is contained in:
Lorenz Meier
2014-12-15 23:33:42 +01:00
parent aa40c69853 89e2e08de0
commit a2bcbabd16
27 changed files with 1422 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
+16
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.autostart
+11
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.vtol_apps
+15
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+39
View File
@@ -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
ROMFS/px4fmu_common/init.d/rcS
+39
View File
@@ -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
#
ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix
+16
View File
@@ -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
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -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
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+38
View File
@@ -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);
src/modules/commander/commander.cpp
+37 -2
View File
@@ -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;
src/modules/fw_att_control/fw_att_control_main.cpp
+115 -44
View File
@@ -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++;
src/modules/mavlink/mavlink_messages.cpp
+12 -1
View File
@@ -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;
src/modules/mc_att_control/mc_att_control_main.cpp
+62 -14
View File
@@ -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);
}
}
}
}
src/modules/sdlog2/sdlog2.c
+25
View File
@@ -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;
src/modules/sdlog2/sdlog2_messages.h
+6 -3
View File
@@ -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"),
src/modules/systemlib/mixer/mixer.h
+1
View File
@@ -466,6 +466,7 @@ public:
OCTA_X,
OCTA_PLUS,
OCTA_COX,
TWIN_ENGINE, /**< VTOL: one engine on each wing */
MAX_GEOMETRY
};
src/modules/systemlib/mixer/mixer_multirotor.cpp
+15 -4
View File
@@ -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;
src/modules/systemlib/mixer/multi_tables
+4 -7
View File
@@ -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]]}
src/modules/uORB/objects_common.cpp
+10
View File
@@ -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);
src/modules/uORB/topics/actuator_controls.h
+3
View File
@@ -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
src/modules/uORB/topics/vehicle_attitude.h
+17
View File
@@ -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 */
};
/**
src/modules/uORB/topics/vehicle_attitude_setpoint.h
+2
View File
@@ -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_ */
src/modules/uORB/topics/vehicle_rates_setpoint.h
+2 -1
View File
@@ -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
src/modules/uORB/topics/vehicle_status.h
+4 -1
View File
@@ -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 {
src/modules/uORB/topics/vtol_vehicle_status.h
+66
View File
@@ -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
src/modules/vtol_att_control/module.mk
+41
View File
@@ -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
src/modules/vtol_att_control/vtol_att_control_main.cpp
+812
View File
File diff suppressed because it is too large Load Diff
src/modules/vtol_att_control/vtol_att_control_params.c
+13
View File
@@ -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);