fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into dev_ros

Browse Source 
Conflicts:
	src/examples/subscriber/subscriber_params.c
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_attitude.h
	src/modules/uORB/topics/vehicle_attitude_setpoint.h
	src/platforms/px4_middleware.h
This commit is contained in:
Thomas Gubler 2014-12-18 12:08:39 +01:00
commit 16618f1ada
34 changed files with 1609 additions and 334 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ROMFS/px4fmu_common/init.d/10018_tbs_endurance Normal file
View File

@ -0,0 +1,31 @@
#!nsh
#
# Team Blacksheep Discovery Long Range Quadcopter
#
# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors
#
# Simon Wilks <simon@px4.io>
#
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.07
param set MC_ROLLRATE_I 0.02
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.4
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_MIN 1200

16
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol Normal file
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

16
ROMFS/px4fmu_common/init.d/rc.autostart
View File

@ -15,6 +15,7 @@
# 10000 .. 10999 Wide arm / H frame
# 11000 .. 11999 Hexa Cox
# 12000 .. 12999 Octo Cox
# 13000 .. 13999 VTOL
#
# Simulation
@ -220,6 +221,11 @@ then
sh /etc/init.d/10017_steadidrone_qu4d
fi
if param compare SYS_AUTOSTART 10018 18
then
sh /etc/init.d/10018_tbs_endurance
fi
#
# Hexa Coaxial
#
@ -237,3 +243,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

15
ROMFS/px4fmu_common/init.d/rc.vtol_apps Normal file
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

39
ROMFS/px4fmu_common/init.d/rc.vtol_defaults Normal file
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

39
ROMFS/px4fmu_common/init.d/rcS
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
#

16
ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix Normal file
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

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -86,6 +86,7 @@ MODULES += modules/position_estimator_inav
#MODULES += modules/fw_att_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control
MODULES += modules/vtol_att_control
#
# Logging

17
msg/vehicle_attitude.msg
View File

@ -16,3 +16,20 @@ float32[4] q # Quaternion (NED)
float32[3] g_comp # Compensated gravity vector
bool R_valid # Rotation matrix valid
bool q_valid # Quaternion valid
# secondary attitude for VTOL
float32 roll_sec # Roll angle (rad, Tait-Bryan, NED)
float32 pitch_sec # Pitch angle (rad, Tait-Bryan, NED)
float32 yaw_sec # Yaw angle (rad, Tait-Bryan, NED)
float32 rollspeed_sec # Roll angular speed (rad/s, Tait-Bryan, NED)
float32 pitchspeed_sec # Pitch angular speed (rad/s, Tait-Bryan, NED)
float32 yawspeed_sec # Yaw angular speed (rad/s, Tait-Bryan, NED)
float32 rollacc_sec # Roll angular accelration (rad/s, Tait-Bryan, NED)
float32 pitchacc_sec # Pitch angular acceleration (rad/s, Tait-Bryan, NED)
float32 yawacc_sec # Yaw angular acceleration (rad/s, Tait-Bryan, NED)
float32[3] rate_offsets_sec # Offsets of the body angular rates from zero
float32[9] R_sec # Rotation matrix, body to world, (Tait-Bryan, NED)
float32[4] q_sec # Quaternion (NED)
float32[3] g_comp_sec # Compensated gravity vector
bool R_valid_sec # Rotation matrix valid
bool q_valid_sec # Quaternion valid

157
msg/vehicle_status.msg Normal file
View File

@ -0,0 +1,157 @@
# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
uint8 MAIN_STATE_MANUAL = 0
uint8 MAIN_STATE_ALTCTL = 1
uint8 MAIN_STATE_POSCTL = 2
uint8 MAIN_STATE_AUTO_MISSION = 3
uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_MAX = 8
# If you change the order, add or remove arming_state_t states make sure to update the arrays
# in state_machine_helper.cpp as well.
uint8 ARMING_STATE_INIT = 0
uint8 ARMING_STATE_STANDBY = 1
uint8 ARMING_STATE_ARMED = 2
uint8 ARMING_STATE_ARMED_ERROR = 3
uint8 ARMING_STATE_STANDBY_ERROR = 4
uint8 ARMING_STATE_REBOOT = 5
uint8 ARMING_STATE_IN_AIR_RESTORE = 6
uint8 ARMING_STATE_MAX = 7
uint8 HIL_STATE_OFF = 0
uint8 HIL_STATE_ON = 1
# Navigation state, i.e. "what should vehicle do".
uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode
uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode
uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6 # RC recover mode
uint8 NAVIGATION_STATE_AUTO_RTGS = 7 # Auto return to groundstation on data link loss
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure
uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down)
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
uint8 NAVIGATION_STATE_LAND = 11 # Land mode
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_MAX = 15
# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128
uint8 VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64
uint8 VEHICLE_MODE_FLAG_HIL_ENABLED = 32
uint8 VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16
uint8 VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8
uint8 VEHICLE_MODE_FLAG_AUTO_ENABLED = 4
uint8 VEHICLE_MODE_FLAG_TEST_ENABLED = 2
uint8 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
# VEHICLE_TYPE, should match 1:1 MAVLink's MAV_TYPE ENUM
uint8 VEHICLE_TYPE_GENERIC = 0 # Generic micro air vehicle.
uint8 VEHICLE_TYPE_FIXED_WING = 1 # Fixed wing aircraft.
uint8 VEHICLE_TYPE_QUADROTOR = 2 # Quadrotor
uint8 VEHICLE_TYPE_COAXIAL = 3 # Coaxial helicopter
uint8 VEHICLE_TYPE_HELICOPTER = 4 # Normal helicopter with tail rotor.
uint8 VEHICLE_TYPE_ANTENNA_TRACKER = 5 # Ground installation
uint8 VEHICLE_TYPE_GCS = 6 # Operator control unit / ground control station
uint8 VEHICLE_TYPE_AIRSHIP = 7 # Airship, controlled
uint8 VEHICLE_TYPE_FREE_BALLOON = 8 # Free balloon, uncontrolled
uint8 VEHICLE_TYPE_ROCKET = 9 # Rocket
uint8 VEHICLE_TYPE_GROUND_ROVER = 10 # Ground rover
uint8 VEHICLE_TYPE_SURFACE_BOAT = 11 # Surface vessel, boat, ship
uint8 VEHICLE_TYPE_SUBMARINE = 12 # Submarine
uint8 VEHICLE_TYPE_HEXAROTOR = 13 # Hexarotor
uint8 VEHICLE_TYPE_OCTOROTOR = 14 # Octorotor
uint8 VEHICLE_TYPE_TRICOPTER = 15 # Octorotor
uint8 VEHICLE_TYPE_FLAPPING_WING = 16 # Flapping wing
uint8 VEHICLE_TYPE_KITE = 17 # Kite
uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18 # Onboard companion controller
uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19 # Vtol with two engines
uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines*/
uint8 VEHICLE_TYPE_ENUM_END = 21
uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage
uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage
# state machine / state of vehicle.
# Encodes the complete system state and is set by the commander app.
uint16 counter # incremented by the writing thread everytime new data is stored
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine
uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state
uint8 hil_state # current hil state
bool failsafe # true if system is in failsafe state
int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum
int32 system_id # system id, inspired by MAVLink's system ID field
int32 component_id # subsystem / component id, inspired by MAVLink's component ID field
bool is_rotary_wing
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_returned_to_home
bool condition_auto_mission_available
bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
bool condition_launch_position_valid # indicates a valid launch position
bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
bool condition_local_position_valid
bool condition_local_altitude_valid
bool condition_airspeed_valid # set to true by the commander app if there is a valid airspeed measurement available
bool condition_landed # true if vehicle is landed, always true if disarmed
bool condition_power_input_valid # set if input power is valid
float32 avionics_power_rail_voltage # voltage of the avionics power rail
bool rc_signal_found_once
bool rc_signal_lost # true if RC reception lost
uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost
bool rc_signal_lost_cmd # true if RC lost mode is commanded
bool rc_input_blocked # set if RC input should be ignored
bool data_link_lost # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
bool engine_failure # Set to true if an engine failure is detected
bool engine_failure_cmd # Set to true if an engine failure mode is commanded
bool gps_failure # Set to true if a gps failure is detected
bool gps_failure_cmd # Set to true if a gps failure mode is commanded
bool barometer_failure # Set to true if a barometer failure is detected
bool offboard_control_signal_found_once
bool offboard_control_signal_lost
bool offboard_control_signal_weak
uint64 offboard_control_signal_lost_interval # interval in microseconds without an offboard control message
bool offboard_control_set_by_command # true if the offboard mode was set by a mavlink command and should not be overridden by RC
# see SYS_STATUS mavlink message for the following
uint32 onboard_control_sensors_present
uint32 onboard_control_sensors_enabled
uint32 onboard_control_sensors_health
float32 load # processor load from 0 to 1
float32 battery_voltage
float32 battery_current
float32 battery_remaining
uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
uint16 drop_rate_comm
uint16 errors_comm
uint16 errors_count1
uint16 errors_count2
uint16 errors_count3
uint16 errors_count4
bool circuit_breaker_engaged_power_check
bool circuit_breaker_engaged_airspd_check
bool circuit_breaker_engaged_enginefailure_check
bool circuit_breaker_engaged_gpsfailure_check

38
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -571,6 +571,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memcpy(&att.R[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], &R_adapted.data[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);

39
src/modules/commander/commander.cpp
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;

159
src/modules/fw_att_control/fw_att_control_main.cpp
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>
*
*/
@ -93,12 +94,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.
*/
@ -113,9 +114,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 */
@ -131,11 +132,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 */
@ -190,6 +195,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 {
@ -229,6 +236,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 */
@ -290,7 +299,7 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main attitude controller collection task.
*/
void task_main();
@ -328,7 +337,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")),
@ -342,6 +351,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_att = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
@ -387,8 +397,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()
@ -463,6 +484,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);
@ -498,7 +520,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) {
@ -530,7 +552,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);
}
}
@ -680,6 +701,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();
@ -697,7 +777,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 {
@ -706,10 +786,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;");
}
@ -821,18 +901,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();
@ -934,20 +1014,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 {
@ -967,28 +1045,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++;

6
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1401,8 +1401,6 @@ FixedwingPositionControl::task_main()
continue;
}
perf_begin(_loop_perf);
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
@ -1421,6 +1419,7 @@ FixedwingPositionControl::task_main()
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
perf_begin(_loop_perf);
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
@ -1475,10 +1474,9 @@ FixedwingPositionControl::task_main()
}
}
perf_end(_loop_perf);
}
perf_end(_loop_perf);
}
_task_running = false;

2
src/modules/fw_pos_control_l1/module.mk
View File

@ -45,3 +45,5 @@ SRCS = fw_pos_control_l1_main.cpp \
mtecs/mTecs_params.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

13
src/modules/mavlink/mavlink_messages.cpp
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;

32
src/modules/mc_att_control/mc_att_control.cpp
View File

@ -97,9 +97,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX);
_params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX);
_params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX);
_params_handles.autostart_id = PX4_PARAM_INIT(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?*/
_is_vtol = true;
}
else {
_is_vtol = false;
}
/*
* do subscriptions
@ -108,9 +116,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
PX4_SUBSCRIBE(_n, parameter_update, MulticopterAttitudeControl::handle_parameter_update, this, 1000);
_parameter_update = PX4_SUBSCRIBE(_n, parameter_update,
MulticopterAttitudeControl::handle_parameter_update, this, 1000);
_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
_v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0);
}
@ -204,7 +214,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
control_attitude(dt);
/* publish the attitude setpoint if needed */
if (_publish_att_sp) {
if (_publish_att_sp && _v_status->get().is_rotary_wing) {
_v_att_sp_mod.timestamp = px4::get_time_micros();
if (_att_sp_pub != nullptr) {
@ -225,7 +235,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
if (_is_vtol) {
_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
}
}
} else {
@ -250,7 +264,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp_pub->publish(_v_rates_sp_mod);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
if (_is_vtol) {
_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
}
}
} else {
@ -277,7 +295,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_actuators_0_pub->publish(_actuators);
} else {
_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
if (_is_vtol) {
_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc);
} else {
_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
}
}
}
}

4
src/modules/mc_att_control/mc_att_control.h
View File

@ -93,6 +93,8 @@ private:
px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */
px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */
bool _is_vtol; /**< true if vehicle is vtol, to be replaced with global API */
px4::NodeHandle _n;
struct {
@ -117,6 +119,8 @@ private:
px4_param_t acro_roll_max;
px4_param_t acro_pitch_max;
px4_param_t acro_yaw_max;
px4_param_t autostart_id;
} _params_handles; /**< handles for interesting parameters */
perf_counter_t _loop_perf; /**< loop performance counter */

2
src/modules/mc_att_control/mc_att_control_base.cpp
View File

@ -266,7 +266,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed->get().armed) {
if (!_armed->get().armed || !_v_status->get().is_rotary_wing) {
_rates_int.zero();
}

10
src/modules/mc_att_control/mc_att_control_base.h
View File

@ -87,11 +87,13 @@ public:
protected:
px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */
px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */
px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */
px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */
px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */
px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */
px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */
px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */
px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */
px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */
px4::PX4_SUBSCRIBER(vehicle_status) *_v_status; /**< vehicle status */
PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint
that gets published eventually */
@ -121,6 +123,8 @@ protected:
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
int32_t autostart_id;
} _params;
bool _publish_att_sp;

25
src/modules/sdlog2/sdlog2.c
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;

9
src/modules/sdlog2/sdlog2_messages.h
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"),

1
src/modules/systemlib/mixer/mixer.h
View File

@ -466,6 +466,7 @@ public:
OCTA_X,
OCTA_PLUS,
OCTA_COX,
TWIN_ENGINE, /**< VTOL: one engine on each wing */
MAX_GEOMETRY
};

19
src/modules/systemlib/mixer/mixer_multirotor.cpp
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;

11
src/modules/systemlib/mixer/multi_tables
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]]}

10
src/modules/uORB/objects_common.cpp
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);

256
src/modules/uORB/topics/vehicle_status.h
View File

@ -1,256 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 - 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 vehicle_status.h
* Definition of the vehicle_status uORB topic.
*
* Published the state machine and the system status bitfields
* (see SYS_STATUS mavlink message), published only by commander app.
*
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app)
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef VEHICLE_STATUS_H_
#define VEHICLE_STATUS_H_
#include <stdint.h>
#include <stdbool.h>
#include <platforms/px4_defines.h>
/**
* @addtogroup topics @{
*/
/**
* Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
*/
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO_MISSION,
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
MAIN_STATE_ACRO,
MAIN_STATE_OFFBOARD,
MAIN_STATE_MAX
} main_state_t;
// If you change the order, add or remove arming_state_t states make sure to update the arrays
// in state_machine_helper.cpp as well.
typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
ARMING_STATE_ARMED,
ARMING_STATE_ARMED_ERROR,
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE,
ARMING_STATE_MAX,
} arming_state_t;
typedef enum {
HIL_STATE_OFF = 0,
HIL_STATE_ON
} hil_state_t;
/**
* Navigation state, i.e. "what should vehicle do".
*/
typedef enum {
NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */
NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */
NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_OFFBOARD,
NAVIGATION_STATE_MAX,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
/**
* Should match 1:1 MAVLink's MAV_TYPE ENUM
*/
enum VEHICLE_TYPE {
VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
VEHICLE_TYPE_KITE = 17, /* Kite | */
VEHICLE_TYPE_ENUM_END = 18, /* | */
};
enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */
VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */
};
/**
* @addtogroup topics
* @{
*/
/**
* state machine / state of vehicle.
*
* Encodes the complete system state and is set by the commander app.
*/
struct vehicle_status_s {
/* use of a counter and timestamp recommended (but not necessary) */
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state; /**< main state machine */
navigation_state_t nav_state; /**< set navigation state machine to specified value */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
bool failsafe; /**< true if system is in failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
bool is_rotary_wing;
bool condition_battery_voltage_valid;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */
bool condition_system_sensors_initialized;
bool condition_system_returned_to_home;
bool condition_auto_mission_available;
bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */
bool condition_launch_position_valid; /**< indicates a valid launch position */
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid;
bool condition_local_altitude_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool condition_power_input_valid; /**< set if input power is valid */
float avionics_power_rail_voltage; /**< voltage of the avionics power rail */
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */
bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool data_link_lost; /**< datalink to GCS lost */
bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */
uint8_t data_link_lost_counter; /**< counts unique data link lost events */
bool engine_failure; /** Set to true if an engine failure is detected */
bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */
bool gps_failure; /** Set to true if a gps failure is detected */
bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */
bool barometer_failure; /** Set to true if a barometer failure is detected */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
and should not be overridden by RC */
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;
uint32_t onboard_control_sensors_health;
float load; /**< processor load from 0 to 1 */
float battery_voltage;
float battery_current;
float battery_remaining;
enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
uint16_t drop_rate_comm;
uint16_t errors_comm;
uint16_t errors_count1;
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
bool circuit_breaker_engaged_power_check;
bool circuit_breaker_engaged_airspd_check;
bool circuit_breaker_engaged_enginefailure_check;
bool circuit_breaker_engaged_gpsfailure_check;
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_status);
#endif

66
src/modules/uORB/topics/vtol_vehicle_status.h Normal file
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

25
src/modules/uORB/uORB.h
View File

@ -264,9 +264,28 @@ __END_DECLS
/* Diverse uORB header defines */ //XXX: move to better location
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
typedef struct actuator_controls_s actuator_controls_0_s;
ORB_DECLARE(actuator_controls_1);
typedef struct actuator_controls_s actuator_controls_1_s;
ORB_DECLARE(actuator_controls_2);
typedef struct actuator_controls_s actuator_controls_2_s;
ORB_DECLARE(actuator_controls_3);
typedef struct actuator_controls_s actuator_controls_3_s;
ORB_DECLARE(actuator_controls_virtual_mc);
typedef struct actuator_controls_s actuator_controls_virtual_mc_s;
ORB_DECLARE(actuator_controls_virtual_fw);
typedef struct actuator_controls_s actuator_controls_virtual_fw_s;
ORB_DECLARE(mc_virtual_rates_setpoint);
typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s;
ORB_DECLARE(fw_virtual_rates_setpoint);
typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s;
ORB_DECLARE(mc_virtual_attitude_setpoint);
typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s;
ORB_DECLARE(fw_virtual_attitude_setpoint);
typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
typedef uint8_t arming_state_t;
typedef uint8_t main_state_t;
typedef uint8_t hil_state_t;
typedef uint8_t navigation_state_t;
#endif /* _UORB_UORB_H */

41
src/modules/vtol_att_control/module.mk Normal file
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

812
src/modules/vtol_att_control/vtol_att_control_main.cpp Normal file
View File

@ -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, &param_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;
}

13
src/modules/vtol_att_control/vtol_att_control_params.c Normal file
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);

2
src/platforms/px4_includes.h
View File

@ -59,6 +59,7 @@
#include <px4/vehicle_control_mode.h>
#include <px4/actuator_armed.h>
#include <px4/parameter_update.h>
#include <px4/parameter_status.h>
#endif
#else
@ -76,6 +77,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>

1
src/platforms/px4_middleware.h
View File

@ -64,7 +64,6 @@ __EXPORT inline bool ok() { return !task_should_exit; }
class Rate
{
public:
/**
* Construct the Rate object and set rate