mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
vehicles: add new vehicle type: Airship (#14862)
Co-authored-by: Anton Erasmus <anton@flycloudline.com> Co-authored-by: Beat Küng <beat-kueng@gmx.net>
This commit is contained in:
committed by
GitHub
parent
454433c2ef
commit
fa4818e467
15
ROMFS/px4fmu_common/init.d-posix/2507_cloudship
Normal file
15
ROMFS/px4fmu_common/init.d-posix/2507_cloudship
Normal file
@@ -0,0 +1,15 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
#
|
||||||
|
# @name Cloudship
|
||||||
|
# @type Airship
|
||||||
|
# @class Airship
|
||||||
|
#
|
||||||
|
# @output MAIN1 thrust tilt
|
||||||
|
# @output MAIN2 starboard thruster
|
||||||
|
# @output MAIN3 port thruster
|
||||||
|
# @output MAIN4 tail thruster
|
||||||
|
|
||||||
|
sh /etc/init.d/rc.airship_defaults
|
||||||
|
|
||||||
|
set MIXER cloudship
|
||||||
|
set PWM_OUT 1234
|
||||||
21
ROMFS/px4fmu_common/init.d/airframes/2507_cloudship
Normal file
21
ROMFS/px4fmu_common/init.d/airframes/2507_cloudship
Normal file
@@ -0,0 +1,21 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
#
|
||||||
|
# @name Cloudship
|
||||||
|
# @type Airship
|
||||||
|
# @class Airship
|
||||||
|
#
|
||||||
|
# @output MAIN1 starboard thruster
|
||||||
|
# @output MAIN2 port thruster
|
||||||
|
# @output MAIN3 thrust tilt
|
||||||
|
# @output MAIN4 tail thruster
|
||||||
|
|
||||||
|
sh /etc/init.d/rc.airship_defaults
|
||||||
|
|
||||||
|
if [ $AUTOCNF = yes ]
|
||||||
|
then
|
||||||
|
param set COM_PREARM_MODE 2
|
||||||
|
param set CBRK_IO_SAFETY 22027
|
||||||
|
fi
|
||||||
|
|
||||||
|
set MIXER cloudship
|
||||||
|
set PWM_OUT 1234
|
||||||
59
ROMFS/px4fmu_common/init.d/rc.airship_apps
Normal file
59
ROMFS/px4fmu_common/init.d/rc.airship_apps
Normal file
@@ -0,0 +1,59 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
#
|
||||||
|
# Standard apps for airships. Attitude/Position estimator, Attitude/Position control.
|
||||||
|
#
|
||||||
|
# NOTE: Script variables are declared/initialized/unset in the rcS script.
|
||||||
|
#
|
||||||
|
|
||||||
|
###############################################################################
|
||||||
|
# Begin Estimator Group Selection #
|
||||||
|
###############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# LPE
|
||||||
|
#
|
||||||
|
if param compare SYS_MC_EST_GROUP 1
|
||||||
|
then
|
||||||
|
#
|
||||||
|
# Try to start LPE. If it fails, start EKF2 as a default.
|
||||||
|
# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
|
||||||
|
#
|
||||||
|
if attitude_estimator_q start
|
||||||
|
then
|
||||||
|
echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
|
||||||
|
local_position_estimator start
|
||||||
|
else
|
||||||
|
echo "ERROR [init] Estimator LPE not available. Using EKF2"
|
||||||
|
param set SYS_MC_EST_GROUP 2
|
||||||
|
param save
|
||||||
|
reboot
|
||||||
|
fi
|
||||||
|
else
|
||||||
|
#
|
||||||
|
# Q estimator (attitude estimation only)
|
||||||
|
#
|
||||||
|
if param compare SYS_MC_EST_GROUP 3
|
||||||
|
then
|
||||||
|
attitude_estimator_q start
|
||||||
|
else
|
||||||
|
#
|
||||||
|
# EKF2
|
||||||
|
#
|
||||||
|
param set SYS_MC_EST_GROUP 2
|
||||||
|
ekf2 start
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
###############################################################################
|
||||||
|
# End Estimator Group Selection #
|
||||||
|
###############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Start Airship Attitude Controller.
|
||||||
|
#
|
||||||
|
airship_att_control start
|
||||||
|
|
||||||
|
#
|
||||||
|
# Start Land Detector.
|
||||||
|
#
|
||||||
|
land_detector start airship
|
||||||
14
ROMFS/px4fmu_common/init.d/rc.airship_defaults
Normal file
14
ROMFS/px4fmu_common/init.d/rc.airship_defaults
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
#
|
||||||
|
# Airship default parameters.
|
||||||
|
#
|
||||||
|
# NOTE: Script variables are declared/initialized/unset in the rcS script.
|
||||||
|
#
|
||||||
|
|
||||||
|
set VEHICLE_TYPE airship
|
||||||
|
|
||||||
|
#
|
||||||
|
# This is the gimbal pass mixer.
|
||||||
|
#
|
||||||
|
set MIXER_AUX pass
|
||||||
|
set PWM_AUX_OUT 1234
|
||||||
@@ -147,6 +147,32 @@ then
|
|||||||
sh /etc/init.d/rc.vtol_apps
|
sh /etc/init.d/rc.vtol_apps
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
#
|
||||||
|
# Airship setup.
|
||||||
|
#
|
||||||
|
if [ $VEHICLE_TYPE = airship ]
|
||||||
|
then
|
||||||
|
if [ $MIXER = none ]
|
||||||
|
then
|
||||||
|
echo "Airship mixer undefined"
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ $MAV_TYPE = none ]
|
||||||
|
then
|
||||||
|
# Set a default MAV_TYPE = 7 if not defined.
|
||||||
|
set MAV_TYPE 7
|
||||||
|
fi
|
||||||
|
|
||||||
|
# Set the mav type parameter.
|
||||||
|
param set MAV_TYPE ${MAV_TYPE}
|
||||||
|
|
||||||
|
# Load mixer and configure outputs.
|
||||||
|
sh /etc/init.d/rc.interface
|
||||||
|
|
||||||
|
# Start airship apps.
|
||||||
|
sh /etc/init.d/rc.airship_apps
|
||||||
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
# UUV setup
|
# UUV setup
|
||||||
#
|
#
|
||||||
|
|||||||
47
ROMFS/px4fmu_common/mixers/cloudship.main.mix
Normal file
47
ROMFS/px4fmu_common/mixers/cloudship.main.mix
Normal file
@@ -0,0 +1,47 @@
|
|||||||
|
Thrust tilt/ Starboard Thrust / Port Thrust / Tail Thrust mixer for PX4FMU
|
||||||
|
=======================================================
|
||||||
|
|
||||||
|
This file defines mixers suitable for controlling an airship with
|
||||||
|
a thrust tilt, starboard and port thruster and a tail thruster using PX4FMU.
|
||||||
|
The configuration assumes the starboard thruster is connected to PX4FMU
|
||||||
|
output 1, port thruster to output 2, tilt servo to output 3, and the
|
||||||
|
tail thruster to output 4.
|
||||||
|
|
||||||
|
Inputs to the mixer come from channel group 0 (vehicle attitude),
|
||||||
|
channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust).
|
||||||
|
|
||||||
|
Starboard and port thruster mixer
|
||||||
|
-----------------
|
||||||
|
Two scalers total (output, thrust).
|
||||||
|
|
||||||
|
By default mixer output is normalized. The input is in the (0 - 1) range.
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
S: 0 3 0 20000 -10000 -10000 10000
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
S: 0 3 0 20000 -10000 -10000 10000
|
||||||
|
|
||||||
|
Servo controlling tilt mixer
|
||||||
|
------------
|
||||||
|
Two scalers total (output, tilt angle).
|
||||||
|
|
||||||
|
This mixer assumes that the tilt servo is set up correctly mechanically;
|
||||||
|
depending on the actual configuration it may be necessary to reverse the scaling
|
||||||
|
factors (to reverse the servo movement) and adjust the offset, scaling and
|
||||||
|
endpoints to suit.
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
S: 0 1 10000 10000 0 -10000 10000
|
||||||
|
|
||||||
|
Tail thruster mixer
|
||||||
|
------------
|
||||||
|
Two scalers total (output, yaw).
|
||||||
|
|
||||||
|
This mixer assumes that the tail thruster is set up correctly mechanically;
|
||||||
|
depending on the actual configuration it may be necessary to reverse the scaling
|
||||||
|
factors (to reverse the motor movement) and adjust the offset, scaling and
|
||||||
|
endpoints to suit.
|
||||||
|
|
||||||
|
M: 1
|
||||||
|
S: 0 2 10000 10000 0 -10000 10000
|
||||||
@@ -90,6 +90,8 @@ class ParameterGroup(object):
|
|||||||
return "YPlus"
|
return "YPlus"
|
||||||
elif (self.name == "Autogyro"):
|
elif (self.name == "Autogyro"):
|
||||||
return "Autogyro"
|
return "Autogyro"
|
||||||
|
elif (self.name == "Airship"):
|
||||||
|
return "Airship"
|
||||||
elif (self.name == "Rover"):
|
elif (self.name == "Rover"):
|
||||||
return "Rover"
|
return "Rover"
|
||||||
elif (self.name == "Boat"):
|
elif (self.name == "Boat"):
|
||||||
|
|||||||
@@ -21,6 +21,7 @@ px4_add_board(
|
|||||||
tone_alarm
|
tone_alarm
|
||||||
#uavcan
|
#uavcan
|
||||||
MODULES
|
MODULES
|
||||||
|
airship_att_control
|
||||||
airspeed_selector
|
airspeed_selector
|
||||||
attitude_estimator_q
|
attitude_estimator_q
|
||||||
camera_feedback
|
camera_feedback
|
||||||
|
|||||||
@@ -53,6 +53,7 @@ uint8 VEHICLE_TYPE_UNKNOWN = 0
|
|||||||
uint8 VEHICLE_TYPE_ROTARY_WING = 1
|
uint8 VEHICLE_TYPE_ROTARY_WING = 1
|
||||||
uint8 VEHICLE_TYPE_FIXED_WING = 2
|
uint8 VEHICLE_TYPE_FIXED_WING = 2
|
||||||
uint8 VEHICLE_TYPE_ROVER = 3
|
uint8 VEHICLE_TYPE_ROVER = 3
|
||||||
|
uint8 VEHICLE_TYPE_AIRSHIP = 4
|
||||||
|
|
||||||
# state machine / state of vehicle.
|
# state machine / state of vehicle.
|
||||||
# Encodes the complete system state and is set by the commander app.
|
# Encodes the complete system state and is set by the commander app.
|
||||||
|
|||||||
@@ -80,7 +80,7 @@ set(models none shell
|
|||||||
if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480
|
if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480
|
||||||
plane plane_cam plane_catapult plane_lidar
|
plane plane_cam plane_catapult plane_lidar
|
||||||
standard_vtol tailsitter tiltrotor
|
standard_vtol tailsitter tiltrotor
|
||||||
rover r1_rover boat
|
rover r1_rover boat cloudship
|
||||||
uuv_hippocampus)
|
uuv_hippocampus)
|
||||||
set(worlds none empty baylands ksql_airport mcmillan_airfield sonoma_raceway warehouse windy)
|
set(worlds none empty baylands ksql_airport mcmillan_airfield sonoma_raceway warehouse windy)
|
||||||
set(all_posix_vmd_make_targets)
|
set(all_posix_vmd_make_targets)
|
||||||
|
|||||||
43
src/modules/airship_att_control/CMakeLists.txt
Normal file
43
src/modules/airship_att_control/CMakeLists.txt
Normal file
@@ -0,0 +1,43 @@
|
|||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
px4_add_module(
|
||||||
|
MODULE modules__airship_att_control
|
||||||
|
MAIN airship_att_control
|
||||||
|
STACK_MAX 3500
|
||||||
|
COMPILE_FLAGS
|
||||||
|
SRCS
|
||||||
|
airship_att_control_main.cpp
|
||||||
|
DEPENDS
|
||||||
|
px4_work_queue
|
||||||
|
)
|
||||||
97
src/modules/airship_att_control/airship_att_control.hpp
Normal file
97
src/modules/airship_att_control/airship_att_control.hpp
Normal file
@@ -0,0 +1,97 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2013-2018 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
#include <uORB/Publication.hpp>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/SubscriptionCallback.hpp>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Airship attitude control app start / stop handling function
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
|
||||||
|
public px4::WorkItem
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
AirshipAttitudeControl();
|
||||||
|
|
||||||
|
virtual ~AirshipAttitudeControl();
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int task_spawn(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int custom_command(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int print_usage(const char *reason = nullptr);
|
||||||
|
|
||||||
|
/** @see ModuleBase::print_status() */
|
||||||
|
int print_status() override;
|
||||||
|
|
||||||
|
void Run() override;
|
||||||
|
|
||||||
|
bool init();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Check for parameter update and handle it.
|
||||||
|
*/
|
||||||
|
void parameter_update_poll();
|
||||||
|
|
||||||
|
void publish_actuator_controls();
|
||||||
|
|
||||||
|
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
|
||||||
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
|
||||||
|
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
|
||||||
|
|
||||||
|
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||||
|
|
||||||
|
uORB::Publication<actuator_controls_s> _actuators_0_pub;
|
||||||
|
|
||||||
|
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
|
||||||
|
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
|
||||||
|
struct actuator_controls_s _actuators {}; /**< actuator controls */
|
||||||
|
|
||||||
|
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||||
|
|
||||||
|
};
|
||||||
213
src/modules/airship_att_control/airship_att_control_main.cpp
Normal file
213
src/modules/airship_att_control/airship_att_control_main.cpp
Normal file
@@ -0,0 +1,213 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2013-2019 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 airship_att_control_main.cpp
|
||||||
|
* Airship attitude controller.
|
||||||
|
*
|
||||||
|
* @author Anton Erasmus <anton@flycloudline.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "airship_att_control.hpp"
|
||||||
|
|
||||||
|
using namespace matrix;
|
||||||
|
|
||||||
|
AirshipAttitudeControl::AirshipAttitudeControl() :
|
||||||
|
ModuleParams(nullptr),
|
||||||
|
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||||
|
_actuators_0_pub(ORB_ID(actuator_controls_0)),
|
||||||
|
_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
AirshipAttitudeControl::~AirshipAttitudeControl()
|
||||||
|
{
|
||||||
|
perf_free(_loop_perf);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
AirshipAttitudeControl::init()
|
||||||
|
{
|
||||||
|
if (!_vehicle_angular_velocity_sub.registerCallback()) {
|
||||||
|
PX4_ERR("vehicle_angular_velocity callback registration failed!");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
AirshipAttitudeControl::parameter_update_poll()
|
||||||
|
{
|
||||||
|
// check for parameter updates
|
||||||
|
if (_parameter_update_sub.updated()) {
|
||||||
|
// clear update
|
||||||
|
parameter_update_s pupdate;
|
||||||
|
_parameter_update_sub.copy(&pupdate);
|
||||||
|
|
||||||
|
// update parameters from storage
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
AirshipAttitudeControl::publish_actuator_controls()
|
||||||
|
{
|
||||||
|
// zero actuators if not armed
|
||||||
|
if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
|
||||||
|
for (uint8_t i = 0 ; i < 4 ; i++) {
|
||||||
|
_actuators.control[i] = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_actuators.control[0] = 0.0f;
|
||||||
|
_actuators.control[1] = _manual_control_sp.x;
|
||||||
|
_actuators.control[2] = _manual_control_sp.r;
|
||||||
|
_actuators.control[3] = _manual_control_sp.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
|
||||||
|
_actuators.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
_actuators_0_pub.publish(_actuators);
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
AirshipAttitudeControl::Run()
|
||||||
|
{
|
||||||
|
if (should_exit()) {
|
||||||
|
_vehicle_angular_velocity_sub.unregisterCallback();
|
||||||
|
exit_and_cleanup();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
perf_begin(_loop_perf);
|
||||||
|
|
||||||
|
/* run controller on gyro changes */
|
||||||
|
vehicle_angular_velocity_s angular_velocity;
|
||||||
|
|
||||||
|
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
|
||||||
|
|
||||||
|
const Vector3f rates{angular_velocity.xyz};
|
||||||
|
|
||||||
|
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
|
||||||
|
|
||||||
|
/* run the rate controller immediately after a gyro update */
|
||||||
|
publish_actuator_controls();
|
||||||
|
|
||||||
|
/* check for updates in manual control topic */
|
||||||
|
if (_manual_control_sp_sub.updated()) {
|
||||||
|
_manual_control_sp_sub.update(&_manual_control_sp);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* check for updates in vehicle status topic */
|
||||||
|
if (_vehicle_status_sub.updated()) {
|
||||||
|
_vehicle_status_sub.update(&_vehicle_status);
|
||||||
|
}
|
||||||
|
|
||||||
|
parameter_update_poll();
|
||||||
|
}
|
||||||
|
|
||||||
|
perf_end(_loop_perf);
|
||||||
|
}
|
||||||
|
|
||||||
|
int AirshipAttitudeControl::task_spawn(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
AirshipAttitudeControl *instance = new AirshipAttitudeControl();
|
||||||
|
|
||||||
|
if (instance) {
|
||||||
|
_object.store(instance);
|
||||||
|
_task_id = task_id_is_work_queue;
|
||||||
|
|
||||||
|
if (instance->init()) {
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_ERR("alloc failed");
|
||||||
|
}
|
||||||
|
|
||||||
|
delete instance;
|
||||||
|
_object.store(nullptr);
|
||||||
|
_task_id = -1;
|
||||||
|
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AirshipAttitudeControl::print_status()
|
||||||
|
{
|
||||||
|
PX4_INFO("Running");
|
||||||
|
|
||||||
|
perf_print_counter(_loop_perf);
|
||||||
|
|
||||||
|
print_message(_actuators);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AirshipAttitudeControl::custom_command(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return print_usage("unknown command");
|
||||||
|
}
|
||||||
|
|
||||||
|
int AirshipAttitudeControl::print_usage(const char *reason)
|
||||||
|
{
|
||||||
|
if (reason) {
|
||||||
|
PX4_WARN("%s\n", reason);
|
||||||
|
}
|
||||||
|
|
||||||
|
PRINT_MODULE_DESCRIPTION(
|
||||||
|
R"DESCR_STR(
|
||||||
|
### Description
|
||||||
|
This implements the airship attitude and rate controller. Ideally it would
|
||||||
|
take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
|
||||||
|
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
|
||||||
|
|
||||||
|
Currently it is feeding the `manual_control_setpoint` topic directly to the actuators.
|
||||||
|
|
||||||
|
### Implementation
|
||||||
|
To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
|
||||||
|
|
||||||
|
)DESCR_STR");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_NAME("airship_att_control", "controller");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND("start");
|
||||||
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int airship_att_control_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return AirshipAttitudeControl::main(argc, argv);
|
||||||
|
}
|
||||||
62
src/modules/land_detector/AirshipLandDetector.cpp
Normal file
62
src/modules/land_detector/AirshipLandDetector.cpp
Normal file
@@ -0,0 +1,62 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2013-2017 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 AirshipLandDetector.cpp
|
||||||
|
* Land detection algorithm for airships
|
||||||
|
*
|
||||||
|
* @author Anton Erasmus <anton@flycloudline.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "AirshipLandDetector.h"
|
||||||
|
|
||||||
|
namespace land_detector
|
||||||
|
{
|
||||||
|
|
||||||
|
bool AirshipLandDetector::_get_ground_contact_state()
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AirshipLandDetector::_get_landed_state()
|
||||||
|
{
|
||||||
|
|
||||||
|
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
|
||||||
|
return true; // If Landing has been requested then say we have landed.
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return !_armed; // If we are armed we are not landed.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace land_detector
|
||||||
60
src/modules/land_detector/AirshipLandDetector.h
Normal file
60
src/modules/land_detector/AirshipLandDetector.h
Normal file
@@ -0,0 +1,60 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2013-2017 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 AirshipLandDetector.h
|
||||||
|
* Land detection implementation for airships.
|
||||||
|
*
|
||||||
|
* @author Anton Erasmus <anton@flycloudline.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "LandDetector.h"
|
||||||
|
|
||||||
|
namespace land_detector
|
||||||
|
{
|
||||||
|
|
||||||
|
class AirshipLandDetector : public LandDetector
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
AirshipLandDetector() = default;
|
||||||
|
~AirshipLandDetector() override = default;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
bool _get_ground_contact_state() override;
|
||||||
|
bool _get_landed_state() override;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace land_detector
|
||||||
@@ -41,6 +41,7 @@ px4_add_module(
|
|||||||
FixedwingLandDetector.cpp
|
FixedwingLandDetector.cpp
|
||||||
VtolLandDetector.cpp
|
VtolLandDetector.cpp
|
||||||
RoverLandDetector.cpp
|
RoverLandDetector.cpp
|
||||||
|
AirshipLandDetector.cpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
hysteresis
|
hysteresis
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -49,6 +49,7 @@
|
|||||||
#include "MulticopterLandDetector.h"
|
#include "MulticopterLandDetector.h"
|
||||||
#include "RoverLandDetector.h"
|
#include "RoverLandDetector.h"
|
||||||
#include "VtolLandDetector.h"
|
#include "VtolLandDetector.h"
|
||||||
|
#include "AirshipLandDetector.h"
|
||||||
|
|
||||||
|
|
||||||
namespace land_detector
|
namespace land_detector
|
||||||
@@ -77,6 +78,9 @@ int LandDetector::task_spawn(int argc, char *argv[])
|
|||||||
} else if (strcmp(argv[1], "rover") == 0) {
|
} else if (strcmp(argv[1], "rover") == 0) {
|
||||||
obj = new RoverLandDetector();
|
obj = new RoverLandDetector();
|
||||||
|
|
||||||
|
} else if (strcmp(argv[1], "airship") == 0) {
|
||||||
|
obj = new AirshipLandDetector();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
print_usage("unknown mode");
|
print_usage("unknown mode");
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
@@ -138,7 +142,7 @@ The module runs periodically on the HP work queue.
|
|||||||
|
|
||||||
PRINT_MODULE_USAGE_NAME("land_detector", "system");
|
PRINT_MODULE_USAGE_NAME("land_detector", "system");
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
|
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
|
||||||
PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover", "Select vehicle type", false);
|
PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover|airship", "Select vehicle type", false);
|
||||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -143,6 +143,25 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} else if (_system_type == MAV_TYPE_AIRSHIP) {
|
||||||
|
/* airship: scale starboard and port throttle to 0..1 and other channels (tilt, tail thruster) to -1..1 */
|
||||||
|
for (unsigned i = 0; i < 16; i++) {
|
||||||
|
if (armed) {
|
||||||
|
if (i < 2) {
|
||||||
|
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
|
||||||
|
msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
|
||||||
|
msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* set 0 for disabled channels */
|
||||||
|
msg.controls[i] = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
|
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user