gg/fts-can
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

status reporting & failsafe logic wip

Browse Source
This commit is contained in:
neo 2024-06-15 11:00:22 +02:00
parent 5d81761acc
commit d87b3077e4
32 changed files with 312 additions and 773 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.idea/runConfigurations/debug_AFTS.xml generated
View File

@ -3,8 +3,6 @@
<openocd version="1" gdb-port="3333" telnet-port="4444" board-config="$PROJECT_DIR$/openocd.cfg" reset-type="RUN" download-type="ALWAYS">
<debugger kind="GDB" isBundled="true" />
</openocd>
<method v="2">
<option name="ToolBeforeRunTask" enabled="true" actionId="Tool_External Tools_build afts" />
</method>
<method v="2" />
</configuration>
</component>

6
.idea/runConfigurations/debug_v6c.xml generated
View File

@ -1,10 +1,8 @@
<component name="ProjectRunConfigurationManager">
<configuration default="false" name="debug v6c" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="fts-can" TARGET_NAME="px4_fmu-v6c_default" version="1" RUN_PATH="$PROJECT_DIR$/build/px4_fmu-v6c_default/px4_fmu-v6c_default.elf">
<configuration default="false" name="debug v6c" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="fts-can" TARGET_NAME="px4_fmu-v6c_default" CONFIG_NAME="px4_fmu-v6c_default" version="1" RUN_PATH="$PROJECT_DIR$/build/px4_fmu-v6c_default/px4_fmu-v6c_default.elf">
<openocd version="1" gdb-port="3333" telnet-port="4444" board-config="$PROJECT_DIR$/openocd.cfg" reset-type="RUN" download-type="UPDATED_ONLY">
<debugger kind="GDB" isBundled="true" />
</openocd>
<method v="2">
<option name="ToolBeforeRunTask" enabled="true" actionId="Tool_External Tools_build v6c" />
</method>
<method v="2" />
</configuration>
</component>

50
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -1,59 +1,9 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
add_subdirectory(airframes)
px4_add_romfs_files(
rc.airship_apps
rc.airship_defaults
rc.autostart_ext
rc.balloon_apps
rc.balloon_defaults
rc.boat_defaults
rc.fw_apps
rc.fw_defaults
rc.heli_defaults
rc.logging
rc.mc_apps
rc.mc_defaults
rcS
rc.sensors
rc.thermal_cal
rc.rover_apps
rc.rover_defaults
rc.uuv_apps
rc.uuv_defaults
rc.vehicle_setup
rc.vtol_apps
rc.vtol_defaults
)

13
ROMFS/px4fmu_common/init.d/airframes/0001_main_fts
View File

@ -8,18 +8,7 @@
# @maintainer GG <germano.girardi@neosystems.it>
#
. ${R}etc/init.d/rc.mc_defaults
# FTS_TYPE = 0 -> MISCONFIGURED MODULE
# FTS_TYPE = 1 -> MAIN FTS MODULE
# FTS_TYPE = 2 -> AUTO FTS MODULE
param set-default FTS_TYPE 1
# FTS_SIM_MODE = 0 -> NORMAL OPERATION
# FTS_SIM_MODE = 1 -> TERMINATION IS NOT EXECUTED BUT SIMULATED VIA MAVLINK MESSAGE & LOGGING
param set-default FTS_SIM_MODE 0
#. ${R}etc/init.d/rc.mc_defaults
# AFTS_ENABLED = 0 -> AFTS MODULE NOT PRESENT OR DISABLED
# AFTS_ENABLED = 1 -> AFTS MODULE PRESENT AND ENABLED

64
ROMFS/px4fmu_common/init.d/rc.airship_apps
View File

@ -1,64 +0,0 @@
#!/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 Control Allocator
#
control_allocator start
#
# Start Airship Attitude Controller.
#
airship_att_control start
#
# Start Land Detector.
#
land_detector start airship

11
ROMFS/px4fmu_common/init.d/rc.airship_defaults
View File

@ -1,11 +0,0 @@
#!/bin/sh
#
# Airship default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE airship
# MAV_TYPE_AIRSHIP 7
param set-default MAV_TYPE 7

11
ROMFS/px4fmu_common/init.d/rc.autostart_ext
View File

@ -1,11 +0,0 @@
#!/bin/sh
#
# External airframe startup script (on SD card)
#
if [ -e ${SDCARD_EXT_PATH}/rc.autostart ]
then
. ${SDCARD_EXT_PATH}/rc.autostart
else
echo "Error: ${SDCARD_EXT_PATH}/rc.autostart does not exist"
fi

42
ROMFS/px4fmu_common/init.d/rc.balloon_apps
View File

@ -1,42 +0,0 @@
#!/bin/sh
#
# Standard apps for balloons
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
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

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

@ -1,16 +0,0 @@
#!/bin/sh
#
# Balloon default parameters
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE fw
# MAV_TYPE_FREE_BALLOON 8
param set-default MAV_TYPE 8
#
# Default parameters for balloon UAVs.
#
param set-default SYS_MC_EST_GROUP 1

21
ROMFS/px4fmu_common/init.d/rc.boat_defaults
View File

@ -1,21 +0,0 @@
#!/bin/sh
#
# UGV default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE rover
# MAV_TYPE_SURFACE_BOAT 11
param set-default MAV_TYPE 11
#
# Default parameters for UGVs.
#
param set-default MIS_TAKEOFF_ALT 0.01
param set-default NAV_ACC_RAD 2
# Temporary.
param set-default NAV_FW_ALT_RAD 1000

34
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

@ -1,34 +0,0 @@
#!/bin/sh
#
# Standard apps for fixed wing
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
ekf2 start &
#
# Start Control Allocator
#
control_allocator start
#
# Start attitude controller.
#
fw_rate_control start
fw_att_control start
fw_pos_control start
airspeed_selector start
#
# Start attitude control auto-tuner
#
fw_autotune_attitude_control start
#
# Start Land Detector.
#
land_detector start fixedwing

55
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -1,55 +0,0 @@
#!/bin/sh
#
# Fixed wing default parameters
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE fw
# MAV_TYPE_FIXED_WING 1
param set-default MAV_TYPE 1
#
# Default parameters for fixed wing UAVs.
#
param set-default COM_POS_FS_DELAY 5
# there is a 2.5 factor applied on the _FS thresholds if for invalidation
param set-default COM_POS_FS_EPH 50
param set-default COM_VEL_FS_EVH 5
param set-default COM_POS_LOW_EPH 50
# Disable preflight disarm to not interfere with external launching
param set-default COM_DISARM_PRFLT -1
param set-default EKF2_ARSP_THR 8
param set-default EKF2_FUSE_BETA 1
param set-default EKF2_GPS_CHECK 21
param set-default EKF2_MAG_ACCLIM 0
param set-default EKF2_MAG_YAWLIM 0
param set-default EKF2_REQ_EPH 10
param set-default EKF2_REQ_EPV 10
param set-default EKF2_REQ_HDRIFT 0.5
param set-default EKF2_REQ_SACC 1
param set-default EKF2_REQ_VDRIFT 1.0
param set-default RTL_TYPE 1
param set-default RTL_RETURN_ALT 100
param set-default RTL_DESCEND_ALT 100
param set-default RTL_LAND_DELAY -1
#
# FW uses L1 distance for acceptance radius.
# Set a smaller NAV_ACC_RAD for vertical acceptance distance.
#
param set-default NAV_ACC_RAD 10
param set-default MIS_TAKEOFF_ALT 25
param set-default MIS_TKO_LAND_REQ 2
#
# FW takeoff acceleration can easily exceed ublox GPS 2G default.
#
param set-default GPS_UBX_DYNMODEL 8

17
ROMFS/px4fmu_common/init.d/rc.heli_defaults
View File

@ -1,17 +0,0 @@
#!/bin/sh
#
# Helicopter default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
# Inherit from mc
. ${R}etc/init.d/rc.mc_defaults
param set-default MAV_TYPE 4
param set-default COM_PREARM_MODE 2
param set-default COM_SPOOLUP_TIME 10
# No need for minimum collective pitch (or airmode) to keep torque authority
param set-default MPC_MANTHR_MIN 0

29
ROMFS/px4fmu_common/init.d/rc.rover_apps
View File

@ -1,29 +0,0 @@
#!/bin/sh
#
# Standard apps for unmanned ground vehicles (UGV).
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
ekf2 start &
#attitude_estimator_q start
#local_position_estimator start
#
# Start Control Allocator
#
control_allocator start
#
# Start attitude controllers.
#
rover_pos_control start
#
# Start Land Detector.
#
land_detector start rover

22
ROMFS/px4fmu_common/init.d/rc.rover_defaults
View File

@ -1,22 +0,0 @@
#!/bin/sh
#
# UGV default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE rover
# MAV_TYPE_GROUND_ROVER 10
param set-default MAV_TYPE 10
# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
param set-default CBRK_AIRSPD_CHK 162128
param set-default MIS_TAKEOFF_ALT 0.01
param set-default NAV_ACC_RAD 2
param set-default NAV_LOITER_RAD 2
# Temporary.
param set-default NAV_FW_ALT_RAD 1000

65
ROMFS/px4fmu_common/init.d/rc.thermal_cal
View File

@ -1,65 +0,0 @@
#!/bin/sh
#
# Thermal Calibration startup script.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set TEMP_COMP_START ""
if param compare -s TC_A_ENABLE 1
then
set TEMP_COMP_START "true"
fi
if param compare -s TC_B_ENABLE 1
then
set TEMP_COMP_START "true"
fi
if param compare -s TC_G_ENABLE 1
then
set TEMP_COMP_START "true"
fi
if [ "x$TEMP_COMP_START" != "x" ]
then
temperature_compensation start
fi
unset TEMP_COMP_START
#
# Determine if a thermal calibration should be started.
#
set TEMP_CALIB_ARGS ""
if param compare SYS_CAL_ACCEL 1
then
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a"
param set SYS_CAL_ACCEL 0
fi
if param compare SYS_CAL_GYRO 1
then
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -g"
param set SYS_CAL_GYRO 0
fi
if param compare SYS_CAL_BARO 1
then
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -b"
param set SYS_CAL_BARO 0
fi
#
# Start the thermal calibration cycle.
#
if [ "x$TEMP_CALIB_ARGS" != "x" ]
then
temperature_compensation calibrate ${TEMP_CALIB_ARGS}
fi
unset TEMP_CALIB_ARGS

31
ROMFS/px4fmu_common/init.d/rc.uuv_apps
View File

@ -1,31 +0,0 @@
#!/bin/sh
#
# Standard apps for uuvs. Attitude/Position estimator, Attitude/Position control.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
###############################################################################
# Begin Estimator Group Selection #
###############################################################################
ekf2 start &
###############################################################################
# End Estimator Group Selection #
###############################################################################
#
# Start Control Allocator
#
control_allocator start
#
# Start UUV Attitude Controller.
#
uuv_att_control start
#
# Start UUV Land Detector.
#
land_detector start rover

14
ROMFS/px4fmu_common/init.d/rc.uuv_defaults
View File

@ -1,14 +0,0 @@
#!/bin/sh
#
# UUV default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE uuv
# MAV_TYPE_SUBMARINE 12
param set-default MAV_TYPE 12
# UUV don't have an airspeed sensor, so disable checks around it
param set-default CBRK_AIRSPD_CHK 162128

71
ROMFS/px4fmu_common/init.d/rc.vehicle_setup
View File

@ -1,71 +0,0 @@
#!/bin/sh
#
# Vehicle configuration setup script.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Fixed wing setup.
#
if [ $VEHICLE_TYPE = fw ]
then
# Start standard fixedwing apps.
. ${R}etc/init.d/rc.fw_apps
fi
#
# Multicopter setup.
#
if [ $VEHICLE_TYPE = mc ]
then
# Start standard multicopter apps.
. ${R}etc/init.d/rc.mc_apps
fi
#
# UGV setup.
#
if [ $VEHICLE_TYPE = rover ]
then
# Start standard UGV apps.
. ${R}etc/init.d/rc.rover_apps
fi
#
# VTOL setup.
#
if [ $VEHICLE_TYPE = vtol ]
then
# Start standard vtol apps.
. ${R}etc/init.d/rc.vtol_apps
fi
#
# Airship setup.
#
if [ $VEHICLE_TYPE = airship ]
then
# Start airship apps.
. ${R}etc/init.d/rc.airship_apps
fi
#
# UUV setup
#
if [ $VEHICLE_TYPE = uuv ]
then
# Start standard vtol apps.
. ${R}etc/init.d/rc.uuv_apps
fi
#
# Generic setup (autostart ID not found).
#
if [ $VEHICLE_TYPE = none ]
then
echo "No autostart ID found"
#ekf2 start &
fi

46
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

@ -1,46 +0,0 @@
#!/bin/sh
#
# Standard apps for vtol: Attitude/Position estimator, Attitude/Position control.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
###############################################################################
# Begin Estimator group selection #
###############################################################################
ekf2 start &
###############################################################################
# End Estimator group selection #
###############################################################################
#
# Start Control Allocator
#
control_allocator start
airspeed_selector start
vtol_att_control start
mc_rate_control start vtol
mc_att_control start vtol
flight_mode_manager start
mc_pos_control start vtol
mc_hover_thrust_estimator start
if param greater -s MC_AT_EN 0
then
mc_autotune_attitude_control start
fi
fw_rate_control start vtol
fw_att_control start vtol
fw_pos_control start vtol
fw_autotune_attitude_control start vtol
# Start Land Detector
# Multicopter for now until we have something for VTOL
#
land_detector start vtol

45
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
View File

@ -1,45 +0,0 @@
#!/bin/sh
#
# VTOL default parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set VEHICLE_TYPE vtol
# MAV_TYPE_VTOL_FIXEDROTOR 22
param set-default MAV_TYPE 22
# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
param set-default COM_POS_FS_EPH 50
param set-default COM_POS_LOW_EPH 50
param set-default MIS_TAKEOFF_ALT 20
param set-default MIS_YAW_TMT 10
param set-default EKF2_ARSP_THR 10
param set-default EKF2_FUSE_BETA 1
param set-default HTE_VXY_THR 2.0
param set-default MIS_TKO_LAND_REQ 2
param set-default MPC_ACC_HOR_MAX 2
param set-default MPC_VEL_MANUAL 5
param set-default MPC_XY_ERR_MAX 5
param set-default MPC_XY_VEL_MAX 8
param set-default MPC_JERK_MAX 4.5
param set-default MPC_YAW_MODE 4
# reduce aggressiveness around roll and yaw axis,
# as VTOLs usually have high intertia and lot af drag due to wings
param set-default MC_ROLL_P 5
param set-default MC_ROLLRATE_MAX 120
param set-default MC_YAW_P 2
param set-default MC_YAWRATE_MAX 120
param set-default MPC_MAN_Y_MAX 90
param set-default RTL_TYPE 1
param set-default WV_EN 1

16
ROMFS/px4fmu_common/init.d/rcS
View File

@ -379,24 +379,22 @@ else
# battery_status start
#fi
# ONLY START SENSORS ON AUTO-FTS MODULE
if param compare SYS_AUTOSTART 2
then
sensors start
commander start
#dshot start
#pwm_out start
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.
. ${R}etc/init.d/rc.vehicle_setup
# Pre-takeoff continuous magnetometer calibration
if param compare -s MBE_ENABLE 1
then
mag_bias_estimator start
fi
fi
fi
#
# Optional board mavlink streams: rc.board_mavlink
#

2
msg/CMakeLists.txt
View File

@ -90,7 +90,6 @@ set(msg_files
FollowTarget.msg
FollowTargetEstimator.msg
FollowTargetStatus.msg
FtsState.msg
GeneratorStatus.msg
GeofenceResult.msg
GimbalDeviceAttitudeStatus.msg
@ -124,6 +123,7 @@ set(msg_files
LogMessage.msg
MagnetometerBiasEstimate.msg
MagWorkerData.msg
MainFtsStatus.msg
ManualControlSetpoint.msg
ManualControlSwitches.msg
MavlinkLog.msg

4
msg/FtsState.msg → msg/MainFtsStatus.msg
View File

@ -1,8 +1,6 @@
uint64 timestamp # time since system start (microseconds)
bool fts_on
uint8 main_fts_state
uint8 ground_fts_state
uint8 auto_fts_state
uint8 main_fts_state

9
src/drivers/fts/fts.cpp
View File

@ -21,7 +21,7 @@ FTS::FTS() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
_fts_state_pub.advertise();
//_main_fts_status_pub.advertise();
}
FTS::~FTS()
@ -89,15 +89,14 @@ void FTS::Run()
update_params();
publish_status();
//publish_status();
}
void FTS::publish_status()
{
fts_state_s status{};
status.fts_on = _fts_on;
main_fts_status_s status{};
status.timestamp = hrt_absolute_time();
_fts_state_pub.publish(status);
_main_fts_status_pub.publish(status);
}
int FTS::start()

4
src/drivers/fts/fts.h
View File

@ -11,7 +11,7 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/fts_state.h>
#include <uORB/topics/main_fts_status.h>
#include <uORB/topics/parameter_update.h>
#include <mathlib/mathlib.h>
@ -94,7 +94,7 @@ private:
bool _fts_initialized = false;
bool _fts_on = false;
uORB::Publication<fts_state_s> _fts_state_pub{ORB_ID(fts_state)};
uORB::Publication<main_fts_status_s> _main_fts_status_pub{ORB_ID(main_fts_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

178
src/modules/fts_control/fts_control.cpp
View File

@ -34,6 +34,8 @@ FTSController::init()
fts_id.uidBytes[2]=guid[2];
fts_id.uidBytes[3]=guid[3];
get_instance()->reset();
// run every 1s
ScheduleOnInterval(1000_ms);
return true;
@ -50,39 +52,68 @@ FTSController::Run() {
perf_begin(_cycle_perf);
perf_count(_interval_perf);
// Update all subscriber
update_topics();
// Update FTS comm status
update_status();
// Update AUTO-FTS status
//TODO manage comm timeout
if (_auto_fts_message_sub.updated()) {
_auto_fts_message_sub.copy(&_auto_fts_message);
//ARMING CONDITIONS ARE MET
switch (_auto_fts_state){
case AUTO_FTS_STATE_ARMED:{
//AFTS IS CURRENTLY ARMED
//TODO ADD GEOFENCE LOADED CHECK ETC
//if (_auto_fts_message.config_status != 0 && _auto_fts_message.geofence_loaded != 0 && _auto_fts_message.gps_lock != 0) {
if (_auto_fts_message.gps_lock != 0) {
arm();
// FIRING CONTITIONS ARE MET
if (_auto_fts_message.geofence_violated != 0) {
fire();
//GEOFENCE VIOLATED, FIRE
_auto_fts_state = AUTO_FTS_STATE_FIRE;
} else {
reset();
//GEOFENCE NOT VIOLATED, REMAIN ARMED AND READY
}
} else {
disarm();
reset();
//FIRING CONDITIONS ARE LOST, DISARM AUTO-FTS
_auto_fts_state = AUTO_FTS_STATE_DISARMED;
}
}break;
case AUTO_FTS_STATE_DISARMED:
{
//AFTS IS NOT CURRENTLY ARMED
//TODO ADD GEOFENCE LOADED CHECK ETC
//if (_auto_fts_message.config_status != 0 && _auto_fts_message.geofence_loaded != 0 && _auto_fts_message.gps_lock != 0) {
if (_auto_fts_message.gps_lock != 0) {
if (_auto_fts_message.geofence_violated != 0) {
// FAILSAFE: CANNOT ARM AUTO-FTS WHEN GEOFENCE IS ALREADY VIOLATED
_auto_fts_state = AUTO_FTS_STATE_ERROR;
} else {
// ARM AUTO-FTS
_auto_fts_state = AUTO_FTS_STATE_ARMED;
}
}else {
// REMAIN DISARMED
}
}break;
case AUTO_FTS_STATE_FIRE:
case AUTO_FTS_STATE_ERROR:
default: {
//DO NOTHING
}break;
}
}
// Update GROUND-FTS status
// Manage communication with GROUND-FTS module
// and update GROUND-FTS status
//TODO manage comm timeout
if (_ground_fts_message_sub.updated()) {
_ground_fts_message_sub.copy(&_ground_fts_message);
if (_ground_fts_message.message == PING) {
//PING COMMAND
if (_ground_fts_message.fts_uid == fts_id.uid) {
//PING RECEIVED, GROUND-FTS IS ARMED
_ground_fts_state = GROUND_FTS_STATE_ARMED;
_ground_fts_response.message = PONG;
_ground_fts_response.fts_uid = fts_id.uid;
@ -95,6 +126,7 @@ FTSController::Run() {
_ground_fts_response.fts_uid = fts_id.uid;
_ground_fts_response_pub.publish(_ground_fts_response);
}
}
if (_emergency_command_sub.updated()) {
@ -103,47 +135,80 @@ FTSController::Run() {
if (_emergency_command.param == fts_id.uid) {
if (_emergency_command.emergency_command == ACTIVATE_FTS) {
//TERMINATE COMMAND
fire();
//TERMINATE COMMAND RECEIVED
if(_ground_fts_state == GROUND_FTS_STATE_ARMED) {
_ground_fts_state = GROUND_FTS_STATE_FIRE;
}else{
// FAILSAFE: CANNOT FIRE GROUND-FTS IF CURRENTLY DISARMED
_ground_fts_state = GROUND_FTS_STATE_ERROR;
}
}
}
// Publish FTS controller state
_fts_state.timestamp = hrt_absolute_time();
}
switch (_ground_fts_state) {
case GROUND_FTS_STATE_DISARMED:{
}break;
case GROUND_FTS_STATE_ARMED:{
}break;
case GROUND_FTS_STATE_FIRE:
case GROUND_FTS_STATE_ERROR:
default:
{
//DO NOTHING
}break;
}
switch (_main_fts_state) {
case MAIN_FTS_STATE_DISARMED:{
if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){
_main_fts_state = MAIN_FTS_STATE_ERROR;
}
}break;
case MAIN_FTS_STATE_ARMED:{
if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){
get_instance()->fire();
}
}break;
case MAIN_FTS_STATE_FIRE:
case MAIN_FTS_STATE_ERROR:
default:
{
//DO NOTHING
}break;
}
// Publish new status
update_topics();
perf_end(_cycle_perf);
}
}
void
FTSController::update_status()
{
// Update FTS state
}
void FTSController::send_fts_state()
{
}
void
FTSController::update_topics()
{
_fts_state_pub.publish(_fts_state);
// Publish FTS controller state
_main_fts_status_msg.timestamp = hrt_absolute_time();
_main_fts_status_msg.main_fts_state = _main_fts_state;
_main_fts_status_msg.ground_fts_state = _ground_fts_state;
_main_fts_status_msg.auto_fts_state = _auto_fts_state;
_main_fts_status_pub.publish(_main_fts_status_msg);
}
bool FTSController::update_param()
{
float fts_comm_timeout_s = 5.0f;
_fts_comm_timeout_threshold_us = (uint64_t)fts_comm_timeout_s * 1000000;
return true;
return 1;
}
int
@ -170,28 +235,45 @@ FTSController::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
void
uint8_t
FTSController::fire()
{
if (_main_fts_state == MAIN_FTS_STATE_ARMED) {
fts_drv.fts_on();
_main_fts_state = MAIN_FTS_STATE_FIRE;
return 0;
}else{
return 1;
}
}
void
uint8_t
FTSController::reset()
{
_main_fts_state = MAIN_FTS_STATE_ARMED;
_ground_fts_state = GROUND_FTS_STATE_DISARMED;
_auto_fts_state = AUTO_FTS_STATE_DISARMED;
fts_drv.fts_off();
fts_drv.fts_arm();
return 0;
}
void
uint8_t
FTSController::arm()
{
fts_drv.fts_arm();
_main_fts_state = MAIN_FTS_STATE_ARMED;
_ground_fts_state = GROUND_FTS_STATE_DISARMED;
_auto_fts_state = AUTO_FTS_STATE_DISARMED;
return 0;
}
void
uint8_t
FTSController::disarm()
{
fts_drv.fts_disarm();
_main_fts_state = MAIN_FTS_STATE_DISARMED;
return 0;
}
int
@ -226,8 +308,9 @@ FTSController::print_status()
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
PX4_INFO("Current FTS state:%u", _fts_state.main_fts_state);
PX4_INFO("Main FTS state:%s", mfts_status_str(_main_fts_state));
PX4_INFO("Ground module state:%s", gfts_status_str(_ground_fts_state));
PX4_INFO("Auto module state:%s", afts_status_str(_auto_fts_state));
return 0;
}
@ -242,7 +325,6 @@ FTSController::print_usage(const char *reason)
R"DESCR_STR(
### Description
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("fts_controller", "system");

99
src/modules/fts_control/fts_control.hpp
View File

@ -13,14 +13,12 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/fts_state.h>
#include <uORB/topics/auto_fts_message.h>
#include <uORB/topics/ground_fts_message.h>
#include <uORB/topics/ground_fts_response.h>
#include <uORB/topics/emergency_command.h>
#include <uORB/topics/main_fts_status.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <drivers/fts/fts.h>
using namespace time_literals;
@ -30,7 +28,7 @@ union FTSIDUnion{
uint8_t uidBytes[4];
};
typedef enum FTS_MESSAGE_TYPE
typedef enum FTS_MESSAGE_TYPE_ENUM
{
PING=1, /* Send from the host to the FTS. | */
PONG=2, /* Send from the FTS to the host. | */
@ -41,7 +39,7 @@ typedef enum FTS_MESSAGE_TYPE
FTS_MESSAGE_TYPE_ENUM_END=4, /* | */
}FTS_MESSAGE_TYPE;
typedef enum MAV_EMERGENCY_COMMAND_TYPE
typedef enum MAV_EMERGENCY_COMMAND_TYPE_ENUM
{
ACTIVATE_FTS=1, /* Stop motors and open the parachute | */
EMERGENCY_LANDING=2, /* Land wherever as soon as possible | */
@ -52,6 +50,30 @@ typedef enum MAV_EMERGENCY_COMMAND_TYPE
MAV_EMERGENCY_COMMAND_TYPE_ENUM_END=7, /* | */
}MAV_EMERGENCY_COMMAND_TYPE;
typedef enum MAIN_FTS_STATE_ENUM
{
MAIN_FTS_STATE_ERROR=0,
MAIN_FTS_STATE_DISARMED=1,
MAIN_FTS_STATE_ARMED=2,
MAIN_FTS_STATE_FIRE=3,
} MAIN_FTS_STATE;
typedef enum GROUND_FTS_STATE_ENUM
{
GROUND_FTS_STATE_ERROR=0,
GROUND_FTS_STATE_DISARMED=1,
GROUND_FTS_STATE_ARMED=2,
GROUND_FTS_STATE_FIRE=3,
} GROUND_FTS_STATE;
typedef enum AUTO_FTS_STATE_ENUM
{
AUTO_FTS_STATE_ERROR=0,
AUTO_FTS_STATE_DISARMED=1,
AUTO_FTS_STATE_ARMED=2,
AUTO_FTS_STATE_FIRE=3,
} AUTO_FTS_STATE;
class FTSController : public ModuleBase<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@ -71,18 +93,58 @@ public:
bool init();
int print_status() override;
inline const char* mfts_status_str(uint8_t state){
switch(state){
default:
case MAIN_FTS_STATE_ERROR:
return "ERROR";
case MAIN_FTS_STATE_DISARMED:
return "DISARMED";
case MAIN_FTS_STATE_ARMED:
return "ARMED";
case MAIN_FTS_STATE_FIRE:
return "FIRE";
}
}
inline const char* gfts_status_str(uint8_t state){
switch(state){
default:
case GROUND_FTS_STATE_ERROR:
return "ERROR";
case GROUND_FTS_STATE_DISARMED:
return "DISARMED";
case GROUND_FTS_STATE_ARMED:
return "ARMED";
case GROUND_FTS_STATE_FIRE:
return "FIRE";
}
}
inline const char* afts_status_str(uint8_t state){
switch(state){
default:
case AUTO_FTS_STATE_ERROR:
return "ERROR";
case AUTO_FTS_STATE_DISARMED:
return "DISARMED";
case AUTO_FTS_STATE_ARMED:
return "ARMED";
case AUTO_FTS_STATE_FIRE:
return "FIRE";
}
}
private:
void Run() override;
void send_fts_state();
void update_topics();
void update_status();
bool update_param();
void fire();
void reset();
void arm();
void disarm();
uint8_t fire();
uint8_t reset();
uint8_t arm();
uint8_t disarm();
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
@ -91,20 +153,27 @@ private:
uORB::SubscriptionData<ground_fts_message_s> _ground_fts_message_sub{ORB_ID(ground_fts_message)};
uORB::SubscriptionData<emergency_command_s> _emergency_command_sub{ORB_ID(emergency_command)};
uORB::Publication<fts_state_s> _fts_state_pub{ORB_ID(fts_state)};
uORB::Publication<main_fts_status_s> _main_fts_status_pub{ORB_ID(main_fts_status)};
uORB::Publication<ground_fts_response_s> _ground_fts_response_pub{ORB_ID(ground_fts_response)};
FTS fts_drv;
hrt_abstime _last_fts_state_update_time{0};
fts_state_s _fts_state{};
// Internal module status
MAIN_FTS_STATE _main_fts_state;
GROUND_FTS_STATE _ground_fts_state;
AUTO_FTS_STATE _auto_fts_state;
// UORB topics published
main_fts_status_s _main_fts_status_msg{};
ground_fts_response_s _ground_fts_response{};
// UORB topics subscribed
auto_fts_message_s _auto_fts_message{};
ground_fts_message_s _ground_fts_message{};
ground_fts_response_s _ground_fts_response{};
emergency_command_s _emergency_command{};
uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz
// Global variable storing the UID of the chip.
union FTSIDUnion fts_id;

14
src/modules/fts_control/fts_control_params.c
View File

@ -44,19 +44,5 @@
* @boolean
* @group FTS Control
*/
PARAM_DEFINE_INT32(FTS_EN, 0);
PARAM_DEFINE_INT32(FTS_TYPE, 1);
PARAM_DEFINE_INT32(FTS_SIM_MODE, 1);
PARAM_DEFINE_INT32(AFTS_ENABLED, 1);
/**
* The timeout for fts_state message
*
* @unit s
* @reboot_required true
* @decimal 1
*
* @group FTS Control
*/
PARAM_DEFINE_FLOAT(FTS_COMM_TM, 10.0f);

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -1840,7 +1840,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
break;
// MODE 13 MFTS TO FC
case MAVLINK_MODE_MFTS_FC:
configure_stream_local("MAIN_FTS_MESSAGE", 3.0f);
configure_stream_local("MAIN_FTS_STATUS", 3.0f);
break;
//MODE 14 MFTS TO GFTS
case MAVLINK_MODE_MFTS_GFTS:

7
src/modules/mavlink/mavlink_messages.cpp
View File

@ -92,6 +92,7 @@
#include "streams/LANDING_TARGET.hpp"
#include "streams/LOCAL_POSITION_NED.hpp"
#include "streams/MAG_CAL_REPORT.hpp"
#include "streams/MAIN_FTS_STATUS.hpp"
#include "streams/MANUAL_CONTROL.hpp"
#include "streams/MOUNT_ORIENTATION.hpp"
#include "streams/NAV_CONTROLLER_OUTPUT.hpp"
@ -489,8 +490,12 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamAftsStatus>(),
#endif // AUTO_FTS_MESSAGE_HPP
#if defined(GROUND_FTS_RESPONSE_HPP)
create_stream_list_item<MavlinkStreamGroundFtsResponse>()
create_stream_list_item<MavlinkStreamGroundFtsResponse>(),
#endif // GROUND_FTS_RESPONSE_HPP
#if defined(MAIN_FTS_STATUS_HPP)
create_stream_list_item<MavlinkStreamMainFtsStatus>()
#endif // MAIN_FTS_STATUS_HPP
};
const char *get_stream_name(const uint16_t msg_id)

61
src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp Normal file
View File

@ -0,0 +1,61 @@
#ifndef MAIN_FTS_MESSAGE_HPP
#define MAIN_FTS_MESSAGE_HPP
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/main_fts_status.h>
#include <px4_platform_common/events.h>
class MavlinkStreamMainFtsStatus : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMainFtsStatus(mavlink); }
static constexpr const char *get_name_static() { return "MAIN_FTS_STATUS"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MAIN_FTS_STATUS; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
uORB::Subscription _main_fts_status_sub{ORB_ID(main_fts_status)};
hrt_abstime _last_reset_time{0};
hrt_abstime _last_update_time{0};
unsigned get_size() override
{
return MAVLINK_MSG_ID_MAIN_FTS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
bool const_rate() override { return true; }
private:
explicit MavlinkStreamMainFtsStatus(Mavlink *mavlink) :
MavlinkStream(mavlink)
{
}
bool send() override
{
mavlink_main_fts_status_t msg{};
set_default_values(msg);
mavlink_msg_main_fts_status_send_struct(_mavlink->get_channel(), &msg);
return true;
}
void set_default_values(mavlink_main_fts_status_t &msg) const
{
msg.main_fts_state = 0;
msg.ground_fts_state = 0;
msg.auto_fts_state = 0;
}
};
#endif