fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 11:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into driver_framework

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-01 12:34:02 +01:00
parent 1e99b04d9c 93bb1ab80f
commit f0a4979da6
109 changed files with 5346 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+26 -3
View File
@@ -59,6 +59,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -122,6 +123,7 @@ private:
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _airspeed_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
@@ -161,6 +163,8 @@ private:
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
airspeed_s _airspeed = {};
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@@ -202,6 +206,8 @@ private:
AttitudeEstimatorQ::AttitudeEstimatorQ() :
_vel_prev(0, 0, 0),
_pos_acc(0, 0, 0),
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
@@ -258,7 +264,7 @@ int AttitudeEstimatorQ::start()
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2100,
2400,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
@@ -299,6 +305,8 @@ void AttitudeEstimatorQ::task_main()
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -334,6 +342,10 @@ void AttitudeEstimatorQ::task_main()
// Update sensors
sensor_combined_s sensors;
int best_gyro = 0;
int best_accel = 0;
int best_mag = 0;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
// Feed validator with recent sensor data
@@ -370,8 +382,6 @@ void AttitudeEstimatorQ::task_main()
}
}
int best_gyro, best_accel, best_mag;
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
@@ -488,6 +498,14 @@ void AttitudeEstimatorQ::task_main()
_mocap_hdg = Rmoc.transposed() * v;
}
// Update airspeed
bool airspeed_updated = false;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
@@ -601,6 +619,9 @@ void AttitudeEstimatorQ::task_main()
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Airspeed - take airspeed measurement directly here as no wind is estimated */
ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
@@ -723,6 +744,8 @@ bool AttitudeEstimatorQ::update(float dt)
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
_q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
src/modules/commander/commander.cpp
+2 -2
View File
@@ -1645,7 +1645,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle really is of type vtol*/
/* Make sure that this is only adjusted if vehicle really is of type vtol */
if (is_vtol(&status)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
status.in_transition_mode = vtol_status.vtol_in_trans_mode;
@@ -2365,7 +2365,7 @@ int commander_thread_main(int argc, char *argv[])
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+3 -1
View File
@@ -856,7 +856,9 @@ void AttitudePositionEstimatorEKF::publishControlState()
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
_ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13];
_ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal();
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
src/modules/ekf_att_pos_estimator/estimator_22states.h
+2
View File
@@ -382,6 +382,8 @@ public:
void get_covariance(float c[28]);
float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
protected:
/**
src/modules/fw_att_control/fw_att_control_main.cpp
+11 -9
View File
@@ -59,6 +59,7 @@
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
@@ -142,6 +143,7 @@ private:
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
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
@@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_rates_sp_id(0),
_actuators_id(0),
_attitude_setpoint_id(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
@@ -1015,15 +1020,12 @@ FixedwingAttitudeControl::task_main()
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+18 -4
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -166,6 +167,8 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
@@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
_tecs_status_pub(nullptr),
_nav_capabilities_pub(nullptr),
/* publication ID */
_attitude_setpoint_id(0),
/* states */
_ctrl_state(),
_att_sp(),
@@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
}
@@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
/* XXX check if radius makes sense here */
src/modules/gpio_led/gpio_led.c
+9 -3
View File
@@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[])
"\t\tr2\tPX4IO RELAY2"
);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
);
@@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[])
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
char *pin_name = "PX4FMU GPIO_EXT1";
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
char pin_name[] = "AUX OUT 1";
#endif
@@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[])
}
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
unsigned int n = strtoul(argv[3], NULL, 10);
if (n >= 1 && n <= 6) {
@@ -207,6 +207,8 @@ void gpio_led_start(FAR void *arg)
char *gpio_dev;
#if defined(PX4IO_DEVICE_PATH)
if (priv->use_io) {
gpio_dev = PX4IO_DEVICE_PATH;
@@ -214,6 +216,10 @@ void gpio_led_start(FAR void *arg)
gpio_dev = PX4FMU_DEVICE_PATH;
}
#else
gpio_dev = PX4FMU_DEVICE_PATH;
#endif
/* open GPIO device */
priv->gpio_fd = open(gpio_dev, 0);
src/modules/land_detector/CMakeLists.txt
+1
View File
@@ -41,6 +41,7 @@ px4_add_module(
LandDetector.cpp
MulticopterLandDetector.cpp
FixedwingLandDetector.cpp
VtolLandDetector.cpp
DEPENDS
platforms__common
)
src/modules/land_detector/FixedwingLandDetector.cpp
+32 -24
View File
@@ -36,6 +36,7 @@
* Land detection algorithm for fixedwings
*
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "FixedwingLandDetector.h"
@@ -48,42 +49,44 @@
FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition( {}),
_airspeedSub(-1),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeed{},
_vehicleStatus{},
_arming{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
_controlStateSub(-1),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeedSub(-1),
_controlState{},
_vehicleStatus{},
_arming{},
_airspeed{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_accel_horz_lp(0.0f),
_landDetectTrigger(0)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
}
void FixedwingLandDetector::initialize()
{
// Subscribe to local position and airspeed data
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
_controlStateSub = orb_subscribe(ORB_ID(control_state));
_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
updateParameterCache(true);
}
void FixedwingLandDetector::updateSubscriptions()
{
orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
}
bool FixedwingLandDetector::update()
@@ -99,29 +102,33 @@ bool FixedwingLandDetector::update()
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);
if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val;
}
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);
if (PX4_ISFINITE(val)) {
_velocity_z_filtered = val;
}
}
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
// a leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses
_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;
}
// crude land detector for fixedwing
if (_velocity_xy_filtered < _params.maxVelocity
&& _velocity_z_filtered < _params.maxClimbRate
&& _airspeed_filtered < _params.maxAirSpeed) {
&& _airspeed_filtered < _params.maxAirSpeed
&& _accel_horz_lp < _params.maxIntVelocity) {
// these conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
@@ -151,5 +158,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force)
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
}
}
src/modules/land_detector/FixedwingLandDetector.h
+9 -6
View File
@@ -42,11 +42,11 @@
#define __FIXED_WING_LAND_DETECTOR_H__
#include "LandDetector.h"
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/airspeed.h>
#include <systemlib/param/param.h>
class FixedwingLandDetector : public LandDetector
@@ -83,28 +83,31 @@ private:
param_t maxVelocity;
param_t maxClimbRate;
param_t maxAirSpeed;
param_t maxIntVelocity;
} _paramHandle;
struct {
float maxVelocity;
float maxClimbRate;
float maxAirSpeed;
float maxIntVelocity;
} _params;
private:
int _vehicleLocalPositionSub; /**< notification of local position */
struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */
int _airspeedSub;
int _controlStateSub; /**< notification of local position */
int _vehicleStatusSub;
int _armingSub;
struct airspeed_s _airspeed;
int _airspeedSub;
struct control_state_s _controlState; /**< the result from local position subscription */
struct vehicle_status_s _vehicleStatus;
struct actuator_armed_s _arming;
struct airspeed_s _airspeed;
int _parameterSub;
float _velocity_xy_filtered;
float _velocity_z_filtered;
float _airspeed_filtered;
float _accel_horz_lp;
uint64_t _landDetectTrigger;
};
src/modules/land_detector/LandDetector.cpp
+4 -3
View File
@@ -57,14 +57,14 @@ _work{} {
LandDetector::~LandDetector()
{
work_cancel(LPWORK, &_work);
work_cancel(HPWORK, &_work);
_taskShouldExit = true;
}
int LandDetector::start()
{
/* schedule a cycle to start things */
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
return 0;
}
@@ -107,10 +107,11 @@ void LandDetector::cycle()
// publish the land detected broadcast
orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
//warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF");
}
if (!_taskShouldExit) {
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
}
}
src/modules/land_detector/MulticopterLandDetector.cpp
+7
View File
@@ -95,6 +95,13 @@ bool MulticopterLandDetector::update()
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
return get_landed_state();
}
bool MulticopterLandDetector::get_landed_state()
{
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
src/modules/land_detector/MulticopterLandDetector.h
+11 -6
View File
@@ -60,24 +60,29 @@ protected:
/**
* @brief polls all subscriptions and pulls any data that has changed
**/
void updateSubscriptions();
virtual void updateSubscriptions();
/**
* @brief Runs one iteration of the land detection algorithm
**/
bool update() override;
virtual bool update() override;
/**
* @brief Initializes the land detection algorithm
**/
void initialize() override;
virtual void initialize() override;
private:
/**
* @brief download and update local parameter cache
**/
void updateParameterCache(const bool force);
virtual void updateParameterCache(const bool force);
/**
* @brief get multicopter landed state
**/
bool get_landed_state();
private:
/**
* @brief Handles for interesting parameters
src/modules/land_detector/VtolLandDetector.cpp
+116
View File
@@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 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 VtolLandDetector.cpp
* Land detection algorithm for vtol
*
* @author Roman Bapst <bapstroma@gmail.com>
*/
#include "VtolLandDetector.h"
#include <drivers/drv_hrt.h>
VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
_paramHandle(),
_params(),
_airspeedSub(-1),
_parameterSub(-1),
_airspeed{},
_was_in_air(false),
_airspeed_filtered(0)
{
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}
void VtolLandDetector::initialize()
{
MulticopterLandDetector::initialize();
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
// download parameters
updateParameterCache(true);
}
void VtolLandDetector::updateSubscriptions()
{
MulticopterLandDetector::updateSubscriptions();
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
}
bool VtolLandDetector::update()
{
updateSubscriptions();
updateParameterCache(false);
// this is returned from the mutlicopter land detector
bool landed = get_landed_state();
// for vtol we additionally consider airspeed
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
_airspeed_filtered = 0.0f;
}
// only consider airspeed if we have been in air before to avoid false
// detections in the case of wind on the ground
if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
landed = false;
}
_was_in_air = !landed;
return landed;
}
void VtolLandDetector::updateParameterCache(const bool force)
{
MulticopterLandDetector::updateParameterCache(force);
bool updated;
parameter_update_s paramUpdate;
orb_check(_parameterSub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
}
if (updated || force) {
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
}
}
src/modules/land_detector/VtolLandDetector.h
+94
View File
@@ -0,0 +1,94 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 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 VtolLandDetector.h
* Land detection algorithm for vtol
*
* @author Roman Bapst <bapstr@gmail.com>
*/
#ifndef __VTOL_LAND_DETECTOR_H__
#define __VTOL_LAND_DETECTOR_H__
#include "MulticopterLandDetector.h"
#include <uORB/topics/airspeed.h>
class VtolLandDetector : public MulticopterLandDetector
{
public:
VtolLandDetector();
private:
/**
* @brief polls all subscriptions and pulls any data that has changed
**/
void updateSubscriptions() override;
/**
* @brief Runs one iteration of the land detection algorithm
**/
bool update() override;
/**
* @brief Initializes the land detection algorithm
**/
void initialize() override;
/**
* @brief download and update local parameter cache
**/
void updateParameterCache(const bool force) override;
/**
* @brief Handles for interesting parameters
**/
struct {
param_t maxAirSpeed;
} _paramHandle;
struct {
float maxAirSpeed;
} _params;
int _airspeedSub;
int _parameterSub;
struct airspeed_s _airspeed;
bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */
float _airspeed_filtered; /**< low pass filtered airspeed */
};
#endif
src/modules/land_detector/land_detector_main.cpp
+4
View File
@@ -53,6 +53,7 @@
#include "FixedwingLandDetector.h"
#include "MulticopterLandDetector.h"
#include "VtolLandDetector.h"
//Function prototypes
static int land_detector_start(const char *mode);
@@ -113,6 +114,9 @@ static int land_detector_start(const char *mode)
} else if (!strcmp(mode, "multicopter")) {
land_detector_task = new MulticopterLandDetector();
} else if (!strcmp(mode, "vtol")) {
land_detector_task = new VtolLandDetector();
} else {
warnx("[mode] must be either 'fixedwing' or 'multicopter'");
return -1;
src/modules/land_detector/land_detector_params.c
+13
View File
@@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
*/
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f);
/**
* Fixedwing max short-term velocity
*
* Maximum velocity integral in flight direction allowed in the landed state (m/s)
*
* @min 2
* @max 10
* @unit m/s
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f);
/**
* Airspeed max
*
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -1750,7 +1750,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VFR_HUD", 20.0f);
configure_stream("ATTITUDE", 100.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("RC_CHANNELS", 5.0f);
configure_stream("RC_CHANNELS", 10.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
src/modules/mavlink/mavlink_receiver.cpp
+1 -2
View File
@@ -1324,8 +1324,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
struct airspeed_s airspeed = {};
float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
// XXX need to fix this
float tas = ias;
float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature);
airspeed.timestamp = timestamp;
airspeed.indicated_airspeed_m_s = ias;
src/modules/mc_pos_control/mc_pos_control_main.cpp
+34 -11
View File
@@ -69,6 +69,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@@ -141,6 +142,8 @@ private:
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
orb_id_t _attitude_setpoint_id;
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
@@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_att_sp_pub(nullptr),
_local_pos_sp_pub(nullptr),
_global_vel_sp_pub(nullptr),
_attitude_setpoint_id(0),
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_vel_x_deriv(this, "VELD"),
@@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
orb_check(_ctrl_state_sub, &updated);
@@ -1141,6 +1153,16 @@ MulticopterPositionControl::task_main()
_run_pos_control = true;
_run_alt_control = true;
// reset the horizontal and vertical position hold flags for non-manual modes
// or if position is not controlled
if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
_pos_hold_engaged = false;
}
if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
_alt_hold_engaged = false;
}
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
@@ -1173,10 +1195,9 @@ MulticopterPositionControl::task_main()
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
} else {
@@ -1600,16 +1621,18 @@ MulticopterPositionControl::task_main()
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
} else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}
src/modules/position_estimator_inav/CMakeLists.txt
+6 -3
View File
@@ -30,18 +30,21 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
set(MODULE_CFLAGS )
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
endif()
px4_add_module(
MODULE modules__position_estimator_inav
MAIN position_estimator_inav
STACK 1200
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
position_estimator_inav_main.c
position_estimator_inav_params.c
inertial_filter.c
position_estimator_inav_main.cpp
position_estimator_inav_params.cpp
inertial_filter.cpp
DEPENDS
platforms__common
)
src/modules/position_estimator_inav/inertial_filter.c → src/modules/position_estimator_inav/inertial_filter.cpp
View File
src/modules/position_estimator_inav/position_estimator_inav_main.c → src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+31 -13
View File
@@ -74,6 +74,7 @@
#include <drivers/drv_hrt.h>
#include <platforms/px4_defines.h>
#include <terrain_estimation/terrain_estimator.h>
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
@@ -95,7 +96,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distan
static const unsigned updates_counter_len = 1000000;
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
int position_estimator_inav_thread_main(int argc, char *argv[]);
@@ -153,7 +154,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = false;
position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300,
position_estimator_inav_thread_main,
(argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL);
return 0;
@@ -398,6 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* wait for initial baro value */
bool wait_baro = true;
TerrainEstimator *terrain_estimator = new TerrainEstimator();
thread_running = true;
while (wait_baro && !thread_should_exit) {
@@ -438,9 +441,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* main loop */
px4_pollfd_struct_t fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
px4_pollfd_struct_t fds[1];
fds[0].fd = vehicle_attitude_sub;
fds[0].events = POLLIN;
while (!thread_should_exit) {
int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
@@ -527,9 +530,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_check(optical_flow_sub, &updated);
orb_check(distance_sensor_sub, &updated2);
/* update lidar separately, needed by terrain estimator */
if (updated2) {
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
}
if (updated && updated2) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
/* calculate time from previous update */
// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
@@ -1045,7 +1052,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += R_gps[j][i] * accel_bias_corr[j];
}
if (isfinite(c)) {
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
@@ -1085,7 +1092,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
@@ -1110,7 +1117,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
@@ -1118,7 +1125,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est, acc[2]);
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1145,7 +1152,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
}
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1164,7 +1171,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_predict(dt, x_est, acc[0]);
inertial_filter_predict(dt, y_est, acc[1]);
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1211,7 +1218,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
}
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1232,6 +1239,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
}
/* run terrain estimator */
terrain_estimator->predict(dt, &att, &sensor, &lidar);
terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att);
if (inav_verbose_mode) {
/* print updates rate */
if (t > updates_counter_start + updates_counter_len) {
@@ -1322,6 +1333,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.eph = eph;
global_pos.epv = epv;
if (terrain_estimator->is_valid()) {
global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
global_pos.terrain_alt_valid = true;
} else {
global_pos.terrain_alt_valid = false;
}
if (vehicle_global_position_pub == NULL) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
src/modules/position_estimator_inav/position_estimator_inav_params.c → src/modules/position_estimator_inav/position_estimator_inav_params.cpp
View File
src/modules/px4iofirmware/CMakeLists.txt
+3 -2
View File
@@ -64,6 +64,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
include_directories(
${include_dirs}
${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer
.
)
link_directories(${link_dirs})
add_definitions(${definitions})
@@ -71,11 +72,9 @@ add_definitions(${definitions})
set(srcs
adc.c
controls.c
dsm.c
px4io.c
registers.c
safety.c
sbus.c
../systemlib/up_cxxinitialize.c
../systemlib/perf_counter.c
mixer.cpp
@@ -86,6 +85,8 @@ set(srcs
../systemlib/pwm_limit/pwm_limit.c
../../lib/rc/st24.c
../../lib/rc/sumd.c
../../lib/rc/sbus.c
../../lib/rc/dsm.c
../../drivers/stm32/drv_hrt.c
../../drivers/stm32/drv_pwm_servo.c
../../drivers/boards/${config_io_board}/px4io_init.c
src/modules/px4iofirmware/controls.c
+7 -4
View File
@@ -46,6 +46,8 @@
#include <systemlib/ppm_decode.h>
#include <rc/st24.h>
#include <rc/sumd.h>
#include <rc/sbus.h>
#include <rc/dsm.h>
#include "px4io.h"
@@ -60,7 +62,8 @@ static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
static int _dsm_fd;
static int _dsm_fd = -1;
int _sbus_fd = -1;
static uint16_t rc_value_override = 0;
@@ -74,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
uint16_t temp_count = r_raw_rc_count;
uint8_t n_bytes = 0;
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS);
if (*dsm_updated) {
r_raw_rc_count = temp_count & 0x7fff;
@@ -158,7 +161,7 @@ controls_init(void)
_dsm_fd = dsm_init("/dev/ttyS0");
/* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
_sbus_fd = sbus_init("/dev/ttyS2", false);
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
@@ -240,7 +243,7 @@ controls_tick()
perf_begin(c_gather_sbus);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
src/modules/px4iofirmware/dsm.c
-527
View File
@@ -1,527 +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 dsm.c
*
* Serial protocol decoder for the Spektrum DSM* family of protocols.
*
* Decodes into the global PPM buffer and updates accordingly.
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
static int dsm_fd = -1; /**< File handle to the DSM UART */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
/**
* Attempt to decode a single channel raw channel datum
*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect dsm frame boundaries by the inter-dsm frame delay.
*
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
* dsm frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a dsm frame.
*
* In the case where byte(s) are dropped from a dsm frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*
* Upon receiving a full dsm frame we attempt to decode it
*
* @param[in] raw 16 bit raw channel value from dsm frame
* @param[in] shift position of channel number in raw data
* @param[out] channel pointer to returned channel number
* @param[out] value pointer to returned channel value
* @return true=raw value successfully decoded
*/
static bool
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{
if (raw == 0xffff) {
return false;
}
*channel = (raw >> shift) & 0xf;
uint16_t data_mask = (1 << shift) - 1;
*value = raw & data_mask;
//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
return true;
}
/**
* Attempt to guess if receiving 10 or 11 bit channel values
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
static void
dsm_guess_format(bool reset)
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
cs10 = 0;
cs11 = 0;
samples = 0;
dsm_channel_shift = 0;
return;
}
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) {
cs10 |= (1 << channel);
}
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) {
cs11 |= (1 << channel);
}
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
}
/* wait until we have seen plenty of frames - 5 should normally be enough */
if (samples++ < 5) {
return;
}
/*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognize.
*
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
* stream, we may want to sniff for longer in some cases when we think we
* are talking to a DSM2 receiver in high-resolution mode (so that we can
* reject it, ideally).
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
0x1ff, /* 9 channels (DX9, etc.) */
0x3ff, /* 10 channels (DX10) */
0x1fff, /* 13 channels (DX10t) */
0x3fff /* 18 channels (DX10) */
};
unsigned votes10 = 0;
unsigned votes11 = 0;
for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {
if (cs10 == masks[i]) {
votes10++;
}
if (cs11 == masks[i]) {
votes11++;
}
}
if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11;
debug("DSM: 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10;
debug("DSM: 10-bit format");
return;
}
/* call ourselves to reset our state ... we have to try again */
debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
dsm_guess_format(true);
}
/**
* Initialize the DSM receive functionality
*
* Open the UART for receiving DSM frames and configure it appropriately
*
* @param[in] device Device name of DSM UART
*/
int
dsm_init(const char *device)
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
// enable power on DSM connector
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
if (dsm_fd >= 0) {
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(dsm_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(dsm_fd, TCSANOW, &t);
/* initialise the decoder */
dsm_partial_frame_count = 0;
dsm_last_rx_time = hrt_absolute_time();
/* reset the format detector */
dsm_guess_format(true);
debug("DSM: ready");
} else {
debug("DSM: open failed");
}
return dsm_fd;
}
/**
* Handle DSM satellite receiver bind mode handler
*
* @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
* @param[in] pulses Number of pulses for dsm_bind_send_pulses command
*/
void
dsm_bind(uint16_t cmd, int pulses)
{
#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
#else
const uint32_t usart1RxAsOutp =
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
if (dsm_fd < 0) {
return;
}
switch (cmd) {
case dsm_bind_power_down:
/*power down DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
POWER_RELAY1(0);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
POWER_SPEKTRUM(0);
#endif
break;
case dsm_bind_power_up:
/*power up DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
POWER_RELAY1(1);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
POWER_SPEKTRUM(1);
#endif
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
/*Set UART RX pin to active output mode*/
stm32_configgpio(usart1RxAsOutp);
break;
case dsm_bind_send_pulses:
/*Pulse RX pin a number of times*/
for (int i = 0; i < pulses; i++) {
up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, true);
}
break;
case dsm_bind_reinit_uart:
/*Restore USART RX pin to RS232 receive mode*/
stm32_configgpio(GPIO_USART1_RX);
break;
}
#endif
}
/**
* Decode the entire dsm frame (all contained channels)
*
* @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned
* @return true=DSM frame successfully decoded, false=no update
*/
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
{
/*
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) {
dsm_guess_format(true);
}
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
/* if we don't know the dsm_frame format, update the guessing state machine */
if (dsm_channel_shift == 0) {
dsm_guess_format(false);
return false;
}
/*
* The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
*
* Each channel is a 16-bit unsigned value containing either a 10-
* or 11-bit channel value and a 4-bit channel number, shifted
* either 10 or 11 bits. The MSB may also be set to indicate the
* second dsm_frame in variants of the protocol where more than
* seven channels are being transmitted.
*/
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) {
continue;
}
/* ignore channels out of range */
if (channel >= PX4IO_RC_INPUT_CHANNELS) {
continue;
}
/* update the decoded channel count */
if (channel >= *num_values) {
*num_values = channel + 1;
}
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
if (dsm_channel_shift == 10) {
value *= 2;
}
/*
* Spektrum scaling is special. There are these basic considerations
*
* * Midpoint is 1520 us
* * 100% travel channels are +- 400 us
*
* We obey the original Spektrum scaling (so a default setup will scale from
* 1100 - 1900 us), but we do not obey the weird 1520 us center point
* and instead (correctly) center the center around 1500 us. This is in order
* to get something useful without requiring the user to calibrate on a digital
* link for no reason.
*/
/* scaled integer for decent accuracy while staying efficient */
value = ((((int)value - 1024) * 1000) / 1700) + 1500;
/*
* Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have.
*
* Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
* but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
*/
switch (channel) {
case 0:
channel = 2;
break;
case 1:
channel = 0;
break;
case 2:
channel = 1;
default:
break;
}
values[channel] = value;
}
/*
* Spektrum likes to send junk in higher channel numbers to fill
* their packets. We don't know about a 13 channel model in their TX
* lines, so if we get a channel count of 13, we'll return 12 (the last
* data index that is stable).
*/
if (*num_values == 13) {
*num_values = 12;
}
if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */
*num_values |= 0x8000;
}
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
return true;
}
/**
* Called periodically to check for input data from the DSM UART
*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect dsm frame boundaries by the inter-dsm frame delay.
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
* dsm frame transmission time is ~1.4ms.
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a dsm frame.
* In the case where byte(s) are dropped from a dsm frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
* Upon receiving a full dsm frame we attempt to decode it.
*
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
* @param[out] n_butes number of bytes read
* @param[out] bytes pointer to the buffer of read bytes
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
{
ssize_t ret;
hrt_abstime now;
/*
*/
now = hrt_absolute_time();
if ((now - dsm_last_rx_time) > 5000) {
if (dsm_partial_frame_count > 0) {
dsm_frame_drops++;
dsm_partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current dsm frame.
*/
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1) {
return false;
} else {
*n_bytes = ret;
*bytes = &dsm_frame[dsm_partial_frame_count];
}
dsm_last_rx_time = now;
/*
* Add bytes to the current dsm frame
*/
dsm_partial_frame_count += ret;
/*
* If we don't have a full dsm frame, return
*/
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a dsm frame. Go ahead and
* decode it.
*/
dsm_partial_frame_count = 0;
return dsm_decode(now, values, num_values);
}
src/modules/px4iofirmware/mixer.cpp
+7 -4
View File
@@ -48,6 +48,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <rc/sbus.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h>
@@ -71,6 +72,8 @@ static bool should_arm_nothrottle = false;
static bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
extern int _sbus_fd;
/* selected control values and count for mixing */
enum mixer_source {
MIX_NONE,
@@ -292,10 +295,10 @@ mixer_tick(void)
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
}
} else if (mixer_servos_armed && should_always_enable_pwm) {
@@ -306,11 +309,11 @@ mixer_tick(void)
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
}
}
src/modules/px4iofirmware/px4io.h
-8
View File
@@ -224,14 +224,6 @@ extern uint16_t adc_measure(unsigned channel);
*/
extern void controls_init(void);
extern void controls_tick(void);
extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_channels);
extern void sbus1_output(uint16_t *values, uint16_t num_values);
extern void sbus2_output(uint16_t *values, uint16_t num_values);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
src/modules/px4iofirmware/registers.c
+1
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_pwm_output.h>
#include <systemlib/systemlib.h>
#include <stm32_pwr.h>
#include <rc/dsm.h>
#include "px4io.h"
#include "protocol.h"
src/modules/px4iofirmware/sbus.c
-368
View File
@@ -1,368 +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 sbus.c
*
* Serial protocol decoder for the Futaba S.bus protocol.
*/
#include <px4_config.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#include "protocol.h"
#include "debug.h"
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
#define SBUS1_FRAME_DELAY 14000
/*
Measured values with Futaba FX-30/R6108SB:
-+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
*/
/* define range mapping here, -+100% -> 1000..2000 */
#define SBUS_RANGE_MIN 200.0f
#define SBUS_RANGE_MAX 1800.0f
#define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
{
if (sbus_fd < 0) {
sbus_fd = open(device, O_RDWR | O_NONBLOCK);
}
if (sbus_fd >= 0) {
struct termios t;
/* 100000bps, even parity, two stop bits */
tcgetattr(sbus_fd, &t);
cfsetspeed(&t, 100000);
t.c_cflag |= (CSTOPB | PARENB);
tcsetattr(sbus_fd, TCSANOW, &t);
/* initialise the decoder */
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
debug("S.Bus: ready");
} else {
debug("S.Bus: open failed");
}
return sbus_fd;
}
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
uint8_t offset = 0;
uint16_t value;
hrt_abstime now;
now = hrt_absolute_time();
if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
last_txframe_time = now;
uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
/* 16 is sbus number of servos/channels minus 2 single bit channels.
* currently ignoring single bit channels. */
for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
/*protect from out of bounds values and limit to 11 bits*/
if (value > 0x07ff) {
value = 0x07ff;
}
while (offset >= 8) {
++byteindex;
offset -= 8;
}
oframe[byteindex] |= (value << (offset)) & 0xff;
oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
offset += 11;
}
write(sbus_fd, oframe, SBUS_FRAME_SIZE);
}
}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
char b = 'B';
write(sbus_fd, &b, 1);
}
bool
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
/*
* The S.bus protocol doesn't provide reliable framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current frame.
*/
ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1) {
return false;
}
last_rx_time = now;
/*
* Add bytes to the current frame
*/
partial_frame_count += ret;
/*
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
partial_frame_count = 0;
return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
}
/*
* S.bus decoder matrix.
*
* Each channel value can come from up to 3 input bytes. Each row in the
* matrix describes up to three bytes, and each entry gives:
*
* - byte offset in the data portion of the frame
* - right shift applied to the data byte
* - mask for the data byte
* - left shift applied to the result into the channel value
*/
struct sbus_bit_pick {
uint8_t byte;
uint8_t rshift;
uint8_t mask;
uint8_t lshift;
};
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) {
sbus_frame_drops++;
return false;
}
switch (frame[24]) {
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : max_values;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
unsigned value = 0;
for (unsigned pick = 0; pick < 3; pick++) {
const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
if (decode->mask != 0) {
unsigned piece = frame[1 + decode->byte];
piece >>= decode->rshift;
piece &= decode->mask;
piece <<= decode->lshift;
value |= piece;
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
chancount = 18;
/* channel 17 (index 16) */
values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
*num_values = chancount;
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true;
*sbus_frame_drop = true;
} else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;
}
return true;
}
src/modules/sdlog2/sdlog2.c
+1 -1
View File
@@ -311,7 +311,7 @@ int sdlog2_main(int argc, char *argv[])
deamon_task = px4_task_spawn_cmd("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
3300,
sdlog2_thread_main,
(char * const *)argv);
src/modules/sensors/sensors.cpp
+1 -1
View File
@@ -851,7 +851,7 @@ Sensors::parameters_update()
} else if (_parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
_parameters.battery_voltage_scaling = 0.0082f;
#elif CONFIG_ARCH_BOARD_AEROCORE
_parameters.battery_voltage_scaling = 0.0063f;
src/modules/systemlib/mixer/CMakeLists.txt
+2 -1
View File
@@ -38,7 +38,7 @@ add_custom_command(OUTPUT mixer_multirotor.generated.h
> mixer_multirotor.generated.h)
add_custom_target(mixer_gen
DEPENDS mixer_multirotor.generated.h)
DEPENDS mixer_multirotor.generated.h multi_tables.py)
px4_add_module(
MODULE modules__systemlib__mixer
@@ -51,5 +51,6 @@ px4_add_module(
mixer_multirotor.generated.h
DEPENDS
platforms__common
mixer_gen
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/systemlib/mixer/mixer_multirotor.cpp
+3
View File
@@ -150,6 +150,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorGeometry::QUAD_X;
} else if (!strcmp(geomname, "4h")) {
geometry = MultirotorGeometry::QUAD_H;
} else if (!strcmp(geomname, "4v")) {
geometry = MultirotorGeometry::QUAD_V;
src/modules/systemlib/mixer/multi_tables.py
+8 -1
View File
@@ -62,6 +62,13 @@ quad_x = [
[135, CW],
]
quad_h = [
[ 45, CW],
[-135, CW],
[-45, CCW],
[135, CCW],
]
quad_plus = [
[ 90, CCW],
[ -90, CCW],
@@ -162,7 +169,7 @@ tri_y = [
]
tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y]
tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y]
def variableName(variable):
for variableName, value in list(globals().items()):
src/modules/uORB/objects_common.cpp
+6 -2
View File
@@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s);
#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/mc_virtual_attitude_setpoint.h"
ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s);
#include "topics/fw_virtual_attitude_setpoint.h"
ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s);
#include "topics/manual_control_setpoint.h"
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
src/modules/uORB/uORB.h
-1
View File
@@ -366,7 +366,6 @@ __END_DECLS
/* Diverse uORB header defines */ //XXX: move to better location
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
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;
src/modules/uavcan/CMakeLists.txt
+1 -1
View File
@@ -41,7 +41,7 @@ add_definitions(
-DUAVCAN_NO_ASSERTIONS
-DUAVCAN_PLATFORM=stm32
-DUAVCAN_STM32_${OS_UPPER}=1
-DUAVCAN_STM32_NUM_IFACES=2
-DUAVCAN_STM32_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_STM32_TIMER_NUMBER=5
)
src/modules/uavcan/uavcan_main.cpp
+7 -2
View File
@@ -533,12 +533,17 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
* fail during initialization.
*/
#ifdef GPIO_CAN1_RX
#if defined(GPIO_CAN1_RX)
stm32_configgpio(GPIO_CAN1_RX);
stm32_configgpio(GPIO_CAN1_TX);
#endif
#endif
#if defined(GPIO_CAN2_RX)
stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
stm32_configgpio(GPIO_CAN2_TX);
#endif
#if !defined(GPIO_CAN1_RX) && !defined(GPIO_CAN2_RX)
# error "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX"
#endif
/*
* CAN driver init
src/modules/vtol_att_control/standard.cpp
+8 -1
View File
@@ -186,6 +186,9 @@ void Standard::update_vtol_state()
void Standard::update_transition_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
@@ -242,11 +245,15 @@ void Standard::update_transition_state()
void Standard::update_mc_state()
{
// do nothing
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Standard::update_fw_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (!_flag_enable_mc_motors) {
set_max_mc(950);
src/modules/vtol_att_control/tailsitter.cpp
+324 -11
View File
@@ -35,18 +35,42 @@
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW
#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc),
_airspeed_tot(0.0f),
_min_front_trans_dur(0.5f),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
_params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS");
_params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND");
_params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
}
@@ -55,23 +79,170 @@ Tailsitter::~Tailsitter()
}
int
Tailsitter::parameters_update()
{
float v;
int l;
/* vtol duration of a front transition */
param_get(_params_handles_tailsitter.front_trans_dur, &v);
_params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
/* vtol front transition phase 2 duration */
param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
_params_tailsitter.front_trans_dur_p2 = v;
/* vtol duration of a back transition */
param_get(_params_handles_tailsitter.back_trans_dur, &v);
_params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* vtol airspeed at which it is ok to switch to fw mode */
param_get(_params_handles_tailsitter.airspeed_trans, &v);
_params_tailsitter.airspeed_trans = v;
/* vtol airspeed at which we start blending mc/fw controls */
param_get(_params_handles_tailsitter.airspeed_blend_start, &v);
_params_tailsitter.airspeed_blend_start = v;
/* vtol lock elevons in multicopter */
param_get(_params_handles_tailsitter.elevons_mc_lock, &l);
_params_tailsitter.elevons_mc_lock = l;
/* avoid parameters which will lead to zero division in the transition code */
_params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur);
if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) {
_params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f;
}
return OK;
}
void Tailsitter::update_vtol_state()
{
// simply switch between the two modes
if (!_attc->is_fixed_wing_requested()) {
_vtol_mode = ROTARY_WING;
parameters_update();
} else {
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting in MC control mode, picking up
* forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
* For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
*/
if (_manual_control_sp->aux1 < 0.0f) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// NOT USED
// failsafe into multicopter mode
//_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
// check if we have reached pitch angle to switch to MC mode
if (_v_att->pitch >= PITCH_TRANSITION_BACK) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
} else { // user switchig to FW mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode
if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans
&& _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) {
_vtol_schedule.flight_mode = FW_MODE;
//_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
/* **LATER*** if pitch is closer to mc (-45>)
* go to transition P1
*/
break;
}
}
// map tailsitter specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case TRANSITION_FRONT_P1:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
case TRANSITION_FRONT_P2:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
}
}
void Tailsitter::update_mc_state()
{
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tailsitter::update_fw_state()
@@ -80,13 +251,137 @@ void Tailsitter::update_fw_state()
set_idle_fw();
flag_idle_mc = false;
}
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
_mc_yaw_weight = 0.0f;
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tailsitter::update_transition_state()
{
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att_sp->yaw_body;
_pitch_transition_start = _v_att_sp->pitch_body;
_throttle_transition = _v_att_sp->thrust;
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
_v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f ,
_pitch_transition_start);
/** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */
if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
_v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition ,
(1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition);
}
// disable mc yaw control once the plane has picked up speed
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
} else {
_mc_yaw_weight = 1.0f;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down
/** no motor switching */
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -
(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f;
}
}
/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/
//_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
/** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/
_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f);
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f);
// throttle value is decreesed
_v_att_sp->thrust = _throttle_transition * 0.9f;
/** keep yaw disabled */
_mc_yaw_weight = 0.0f;
/** smoothly move control weight to MC */
_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params_tailsitter.back_trans_dur * 1000000.0f);
_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
(_params_tailsitter.back_trans_dur * 1000000.0f);
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
// compute desired attitude and thrust setpoint for the transition
_v_att_sp->timestamp = hrt_absolute_time();
_v_att_sp->roll_body = 0.0f;
_v_att_sp->yaw_body = _yaw_transition;
_v_att_sp->R_valid = true;
math::Matrix<3, 3> R_sp;
R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
}
void Tailsitter::update_external_state()
{
@@ -110,8 +405,7 @@ void Tailsitter::calc_tot_airspeed()
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
Tailsitter::scale_mc_output()
void Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
@@ -167,8 +461,7 @@ void Tailsitter::fill_actuator_outputs()
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
_actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
@@ -192,6 +485,26 @@ void Tailsitter::fill_actuator_outputs()
break;
case TRANSITION:
// in transition engines are mixed by weight (BACK TRANSITION ONLY)
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
// **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
break;
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
src/modules/vtol_att_control/tailsitter.h
+83 -5
View File
@@ -35,6 +35,7 @@
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
@@ -42,7 +43,9 @@
#define TAILSITTER_H
#include "vtol_type.h"
#include <systemlib/perf_counter.h>
#include <systemlib/perf_counter.h> /** is it necsacery? **/
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Tailsitter : public VtolType
{
@@ -51,21 +54,96 @@ public:
Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter();
/**
* Update vtol state.
*/
void update_vtol_state();
/**
* Update multicopter state.
*/
void update_mc_state();
/**
* Update fixed wing state.
*/
void update_fw_state();
/**
* Update transition state.
*/
void update_transition_state();
/**
* Update external state.
*/
void update_external_state();
private:
void fill_actuator_outputs();
void calc_tot_airspeed();
void scale_mc_output();
float _airspeed_tot;
struct {
float front_trans_dur; /**< duration of first part of front transition */
float front_trans_dur_p2;
float back_trans_dur; /**< duration of back transition */
float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */
float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */
int elevons_mc_lock; /**< lock elevons in multicopter mode */
} _params_tailsitter;
struct {
param_t front_trans_dur;
param_t front_trans_dur_p2;
param_t back_trans_dur;
param_t airspeed_trans;
param_t airspeed_blend_start;
param_t elevons_mc_lock;
} _params_handles_tailsitter;
enum vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */
TRANSITION_BACK, /**< vtol is in back transition mode */
FW_MODE /**< vtol is in fixed wing mode */
};
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
float _airspeed_tot; /** speed estimation for propwash controlled surfaces */
/** not sure about it yet ?! **/
float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
/** should this anouncement stay? **/
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/**
* Speed estimation for propwash controlled surfaces.
*/
void calc_tot_airspeed();
/** is this one still needed? */
void scale_mc_output();
/**
* Write control values to actuator output topics.
*/
void fill_actuator_outputs();
/**
* Update parameters.
*/
int parameters_update();
};
#endif
src/modules/vtol_att_control/tailsitter_params.c
+56
View File
@@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2015 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 tailsitter_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* Duration of front transition phase 2
*
* Time in seconds it should take for the rotors to rotate forward completely from the point
* when the plane has picked up enough airspeed and is ready to go into fixed wind mode.
*
* @min 0.1
* @max 2
* @group VTOL Attitude Control
PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/
src/modules/vtol_att_control/tiltrotor.cpp
+18
View File
@@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
@@ -250,6 +252,9 @@ void Tiltrotor::update_vtol_state()
void Tiltrotor::update_mc_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// make sure motors are not tilted
_tilt_control = _params_tiltrotor.tilt_mc;
@@ -271,6 +276,9 @@ void Tiltrotor::update_mc_state()
void Tiltrotor::update_fw_state()
{
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
@@ -292,6 +300,13 @@ void Tiltrotor::update_fw_state()
void Tiltrotor::update_transition_state()
{
if (!_flag_was_in_trans_mode) {
// save desired heading for transition and last thrust value
_yaw_transition = _v_att->yaw;
_throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
_flag_was_in_trans_mode = true;
}
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// for the first part of the transition the rear rotors are enabled
if (_rear_motors != ENABLED) {
@@ -354,6 +369,9 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
}
void Tiltrotor::update_external_state()
src/modules/vtol_att_control/vtol_att_control_main.cpp
+91 -1
View File
@@ -41,6 +41,7 @@
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#include "vtol_att_control_main.h"
@@ -60,6 +61,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
//init subscription handlers
_v_att_sub(-1),
_v_att_sp_sub(-1),
_mc_virtual_att_sp_sub(-1),
_fw_virtual_att_sp_sub(-1),
_mc_virtual_v_rates_sp_sub(-1),
_fw_virtual_v_rates_sp_sub(-1),
_v_control_mode_sub(-1),
@@ -75,13 +78,16 @@ VtolAttitudeControl::VtolAttitudeControl() :
_actuators_0_pub(nullptr),
_actuators_1_pub(nullptr),
_vtol_vehicle_status_pub(nullptr),
_v_rates_sp_pub(nullptr)
_v_rates_sp_pub(nullptr),
_v_att_sp_pub(nullptr)
{
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(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp));
memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_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));
@@ -271,6 +277,36 @@ VtolAttitudeControl::vehicle_airspeed_poll()
}
}
/**
* Check for attitude set points update.
*/
void
VtolAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
/**
* Check for attitude update.
*/
void
VtolAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
}
}
/**
* Check for battery updates.
*/
@@ -319,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll()
}
/**
* Check for mc virtual attitude setpoint updates.
*/
void
VtolAttitudeControl::mc_virtual_att_sp_poll()
{
bool updated;
orb_check(_mc_virtual_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp);
}
}
/**
* Check for fw virtual attitude setpoint updates.
*/
void
VtolAttitudeControl::fw_virtual_att_sp_poll()
{
bool updated;
orb_check(_fw_virtual_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp);
}
}
/**
* Check for command updates.
*/
@@ -439,6 +507,18 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
_v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust;
}
void VtolAttitudeControl::publish_att_sp()
{
if (_v_att_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
} else {
/* advertise and publish */
_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
}
}
void
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@@ -452,9 +532,12 @@ void VtolAttitudeControl::task_main()
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_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_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_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));
@@ -523,9 +606,13 @@ void VtolAttitudeControl::task_main()
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
mc_virtual_att_sp_poll();
fw_virtual_att_sp_poll();
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.
vehicle_attitude_setpoint_poll();//Check for changes in attitude set points
vehicle_attitude_poll(); //Check for changes in attitude
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();
@@ -582,6 +669,7 @@ void VtolAttitudeControl::task_main()
} else if (_vtol_type->get_mode() == TRANSITION) {
// vehicle is doing a transition
_vtol_vehicle_status.vtol_in_trans_mode = true;
_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition
bool got_new_data = false;
@@ -599,6 +687,7 @@ void VtolAttitudeControl::task_main()
if (got_new_data) {
_vtol_type->update_transition_state();
fill_mc_att_rates_sp();
publish_att_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
@@ -606,6 +695,7 @@ void VtolAttitudeControl::task_main()
_vtol_type->update_external_state();
}
publish_att_sp();
_vtol_type->fill_actuator_outputs();
/* Only publish if the proper mode(s) are enabled */
src/modules/vtol_att_control/vtol_att_control_main.h
+34 -16
View File
@@ -41,6 +41,7 @@
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#ifndef VTOL_ATT_CONTROL_MAIN_H
@@ -62,6 +63,8 @@
#include <drivers/drv_pwm_output.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
@@ -104,22 +107,24 @@ public:
int start(); /* start the task and return OK on success */
bool is_fixed_wing_requested();
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_armed_s *get_armed() {return &_armed;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct battery_status_s *get_batt_status() {return &_batt_status;}
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_armed_s *get_armed() {return &_armed;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct battery_status_s *get_batt_status() {return &_batt_status;}
struct Params *get_params() {return &_params;}
@@ -132,6 +137,8 @@ private:
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_att_sp_sub;
int _fw_virtual_att_sp_sub;
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
@@ -151,9 +158,12 @@ private:
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
orb_advert_t _v_att_sp_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint
struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw 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
@@ -187,6 +197,9 @@ private:
param_t elevons_mc_lock;
} _params_handles;
/* 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. */
int _transition_command;
VtolType *_vtol_type; // base class for different vtol types
@@ -202,12 +215,16 @@ private:
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 mc_virtual_att_sp_poll();
void fw_virtual_att_sp_poll();
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_local_pos_poll(); // Check for changes in sensor values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates.
void vehicle_attitude_poll(); //Check for attitude updates.
void vehicle_battery_poll(); // Check for battery updates
void vehicle_cmd_poll();
void parameters_update_poll(); //Check if parameters have changed
@@ -215,6 +232,7 @@ private:
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
void handle_command();
void publish_att_sp();
};
#endif
src/modules/vtol_att_control/vtol_type.cpp
+2
View File
@@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
_v_rates_sp = _attc->get_rates_sp();
_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
src/modules/vtol_att_control/vtol_type.h
+14 -7
View File
@@ -93,20 +93,22 @@ protected:
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw 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 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 vehicle_local_position_s *_local_pos;
struct airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct Params *_params;
@@ -116,6 +118,11 @@ protected:
float _mc_pitch_weight; // weight for multicopter attitude controller pitch output
float _mc_yaw_weight; // weight for multicopter attitude controller yaw output
float _yaw_transition; // yaw angle in which transition will take place
float _pitch_transition_start; // pitch angle at the start of transition (tailsitter)
float _throttle_transition; // throttle value used for the transition phase
bool _flag_was_in_trans_mode; // true if mode has just switched to transition
};
#endif