diff --git a/.idea/runConfigurations/debug_AFTS.xml b/.idea/runConfigurations/debug_AFTS.xml index 7082621..241cf53 100644 --- a/.idea/runConfigurations/debug_AFTS.xml +++ b/.idea/runConfigurations/debug_AFTS.xml @@ -3,8 +3,6 @@ - - + \ No newline at end of file diff --git a/.idea/runConfigurations/debug_v6c.xml b/.idea/runConfigurations/debug_v6c.xml index cc37ca1..82513ea 100644 --- a/.idea/runConfigurations/debug_v6c.xml +++ b/.idea/runConfigurations/debug_v6c.xml @@ -1,10 +1,8 @@ - + - - + \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 3f19c22..9f0e848 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -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 ) diff --git a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts index 2623ec9..750b3d0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts @@ -8,18 +8,7 @@ # @maintainer GG # -. ${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 diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps deleted file mode 100644 index 837a3a2..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults deleted file mode 100644 index 2b25850..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_ext b/ROMFS/px4fmu_common/init.d/rc.autostart_ext deleted file mode 100644 index 4291a3d..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart_ext +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_apps b/ROMFS/px4fmu_common/init.d/rc.balloon_apps deleted file mode 100644 index 2adb216..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_apps +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults deleted file mode 100644 index 6f7d925..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_defaults deleted file mode 100644 index dbce642..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.boat_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps deleted file mode 100644 index f3c5592..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults deleted file mode 100644 index 77b10d8..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults deleted file mode 100644 index 42d2aca..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps deleted file mode 100644 index 4d04b53..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults deleted file mode 100644 index c4cd9d3..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.thermal_cal b/ROMFS/px4fmu_common/init.d/rc.thermal_cal deleted file mode 100644 index 484b9af..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.thermal_cal +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps deleted file mode 100644 index d2eb132..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_apps +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults deleted file mode 100644 index 344cfff..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup deleted file mode 100644 index 4eb568e..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps deleted file mode 100644 index fdc16af..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults deleted file mode 100644 index 2c273e9..0000000 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index eaf78fb..d7345f6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -379,23 +379,21 @@ else # battery_status start #fi - sensors start - commander start + # ONLY START SENSORS ON AUTO-FTS MODULE + if param compare SYS_AUTOSTART 2 + then + sensors start + commander start - #dshot start - #pwm_out start + # Pre-takeoff continuous magnetometer calibration + if param compare -s MBE_ENABLE 1 + then + mag_bias_estimator start + fi + + fi 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 # # Optional board mavlink streams: rc.board_mavlink diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b7e7ada..0d5de5f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/FtsState.msg b/msg/MainFtsStatus.msg similarity index 90% rename from msg/FtsState.msg rename to msg/MainFtsStatus.msg index a1afe23..5124be3 100644 --- a/msg/FtsState.msg +++ b/msg/MainFtsStatus.msg @@ -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 - diff --git a/src/drivers/fts/fts.cpp b/src/drivers/fts/fts.cpp index c550d09..e787d46 100644 --- a/src/drivers/fts/fts.cpp +++ b/src/drivers/fts/fts.cpp @@ -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() diff --git a/src/drivers/fts/fts.h b/src/drivers/fts/fts.h index bbbc59b..4e74880 100644 --- a/src/drivers/fts/fts.h +++ b/src/drivers/fts/fts.h @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include @@ -94,7 +94,7 @@ private: bool _fts_initialized = false; bool _fts_on = false; - uORB::Publication _fts_state_pub{ORB_ID(fts_state)}; + uORB::Publication _main_fts_status_pub{ORB_ID(main_fts_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index ba49c4f..0645004 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/src/modules/fts_control/fts_control.cpp @@ -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) { + // FIRING CONTITIONS ARE MET + if (_auto_fts_message.geofence_violated != 0) { + //GEOFENCE VIOLATED, FIRE + _auto_fts_state = AUTO_FTS_STATE_FIRE; + } else { + //GEOFENCE NOT VIOLATED, REMAIN ARMED AND READY + } + } else { + //FIRING CONDITIONS ARE LOST, DISARM AUTO-FTS + _auto_fts_state = AUTO_FTS_STATE_DISARMED; + } + }break; - if (_auto_fts_message.gps_lock != 0) { - arm(); + 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; - if (_auto_fts_message.geofence_violated != 0) { - fire(); - } else { - reset(); - } - - } else { - disarm(); - reset(); + case AUTO_FTS_STATE_FIRE: + case AUTO_FTS_STATE_ERROR: + default: { + //DO NOTHING + }break; } } - // Update GROUND-FTS status - if (_ground_fts_message_sub.updated()) { + // 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(); - - perf_end(_cycle_perf); } -} -void -FTSController::update_status() -{ + switch (_ground_fts_state) { + case GROUND_FTS_STATE_DISARMED:{ - // Update FTS state + }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::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() { - fts_drv.fts_on(); + 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"); diff --git a/src/modules/fts_control/fts_control.hpp b/src/modules/fts_control/fts_control.hpp index 08d8d23..e02e11e 100644 --- a/src/modules/fts_control/fts_control.hpp +++ b/src/modules/fts_control/fts_control.hpp @@ -13,14 +13,12 @@ #include #include #include -#include #include #include #include #include +#include #include -#include -#include #include 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. | */ @@ -39,9 +37,9 @@ typedef enum FTS_MESSAGE_TYPE same message as a response. Then the host shall register the FTS_UID field in the response. | */ FTS_MESSAGE_TYPE_ENUM_END=4, /* | */ -} FTS_MESSAGE_TYPE; +}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 | */ @@ -50,7 +48,31 @@ typedef enum MAV_EMERGENCY_COMMAND_TYPE UNHOLD=5, /* Hold position in multi copter mode | */ DISACTIVATE_FTS=6, /* re-enable motors and restore parachute output (testing only) | */ MAV_EMERGENCY_COMMAND_TYPE_ENUM_END=7, /* | */ -} MAV_EMERGENCY_COMMAND_TYPE; +}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, public ModuleParams, public px4::ScheduledWorkItem { @@ -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_sub{ORB_ID(ground_fts_message)}; uORB::SubscriptionData _emergency_command_sub{ORB_ID(emergency_command)}; - uORB::Publication _fts_state_pub{ORB_ID(fts_state)}; - uORB::Publication _ground_fts_response_pub{ORB_ID(ground_fts_response)}; + uORB::Publication _main_fts_status_pub{ORB_ID(main_fts_status)}; + uORB::Publication _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; diff --git a/src/modules/fts_control/fts_control_params.c b/src/modules/fts_control/fts_control_params.c index f49cf8a..1e8176c 100644 --- a/src/modules/fts_control/fts_control_params.c +++ b/src/modules/fts_control/fts_control_params.c @@ -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); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9ae4302..2094c14 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 39cc8ac..b5ce0ed 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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(), #endif // AUTO_FTS_MESSAGE_HPP #if defined(GROUND_FTS_RESPONSE_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // GROUND_FTS_RESPONSE_HPP +#if defined(MAIN_FTS_STATUS_HPP) + create_stream_list_item() +#endif // MAIN_FTS_STATUS_HPP + }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp b/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp new file mode 100644 index 0000000..d622a2d --- /dev/null +++ b/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp @@ -0,0 +1,61 @@ +#ifndef MAIN_FTS_MESSAGE_HPP +#define MAIN_FTS_MESSAGE_HPP + +#include +#include +#include + +#include +#include +#include + +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