From ed14151734919e81c9303a59adb1ba258735f621 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Tue, 19 Jul 2022 09:11:44 +0200 Subject: [PATCH] Update Flight-gear bridge, Add support of TF-G2 autogyro flight-gear model (#19122) * Add Transfer of RPM from FG to PX4, -switch FG_bridge module to ThudnderFlyaerospace * Add TF-G2 flightgear sim target * Add simulator support, fix astyle * Update SITL TF-G2 airframe, update fg bridge Co-authored-by: Vit Hanousek --- .../init.d-posix/airframes/17002_tf-g2 | 55 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + platforms/posix/cmake/sitl_target.cmake | 1 + src/modules/simulator/simulator.h | 4 ++ src/modules/simulator/simulator_mavlink.cpp | 11 ++++ 5 files changed, 72 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 new file mode 100644 index 0000000000..e593991ae8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2 @@ -0,0 +1,55 @@ +#!/bin/sh +# +# @name ThunderFly TF-G2 +# ThunderFly TF-G2 autogyro airframe. Only for FlightGear simulator +# +# @type Autogyro +# @class Autogyro +# +# @url https://github.com/ThunderFly-aerospace/TF-G2/ +# +# + +. ${R}etc/init.d/rc.fw_defaults + +param set-default FW_AIRSPD_STALL 5 + +param set-default FW_P_RMAX_NEG 20.0 +param set-default FW_W_RMAX 10 +param set-default FW_W_EN 1 + +param set-default FW_RR_P 0.08 + +param set-default MIS_LTRMIN_ALT 50 +param set-default MIS_TAKEOFF_ALT 7 + +param set-default NAV_ACC_RAD 20 +param set-default NAV_DLL_ACT 2 +param set-default NAV_LOITER_RAD 50 + +param set-default RWTO_TKOFF 0 +# Parameters related to autogyro takeoff PR +#param set-default AG_TKOFF 1 +#param set-default AG_PROT_TYPE 1 +#param set-default AG_PROT_MIN_RPM 50.0 +#param set-default AG_PROT_TRG_RPM 900.0 +#param set-defoult AG_ROTOR_RPM 900.0 + +param set-default FW_ARSP_SCALE_EN 0 + +param set-default FW_AIRSPD_MAX 35 +param set-default FW_AIRSPD_MIN 7 + +param set-default FW_P_LIM_MAX 25 +param set-default FW_P_LIM_MIN -5 +param set-default FW_R_LIM 30 + +param set-default FW_MAN_P_MAX 30.0 +param set-default FW_MAN_R_MAX 30.0 + +param set-default FW_THR_CRUISE 0.8 +param set-default FW_THR_IDLE 0 +param set-default COM_DISARM_PRFLT 0 + +set MIXER_FILE etc/mixers-sitl/autogyro_sitl.main.mix +set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index fba035b61e..ce1b3d05f4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -78,6 +78,7 @@ px4_add_romfs_files( 3010_quadrotor_x 3011_hexarotor_x 17001_tf-g1 + 17002_tf-g2 2507_cloudship 6011_typhoon_h480 6011_typhoon_h480.post diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index a71e3efbcf..6fbecfc14f 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -394,6 +394,7 @@ if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no") rascal rascal-electric tf-g1 + tf-g2 tf-r1 ) set(all_posix_vmd_make_targets) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 4c0964f1c3..951e33e83e 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -76,6 +76,7 @@ #include #include #include +#include #include @@ -251,6 +252,9 @@ private: uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; + //rpm + uORB::Publication _rpm_pub{ORB_ID(rpm)}; + // HIL GPS static constexpr int MAX_GPS = 3; uORB::PublicationMulti *_sensor_gps_pubs[MAX_GPS] {}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index f133bbadae..d3d749c913 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -424,6 +424,17 @@ void Simulator::handle_message(const mavlink_message_t *msg) case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: handle_message_hil_state_quaternion(msg); break; + + case MAVLINK_MSG_ID_RAW_RPM: + mavlink_raw_rpm_t rpm; + mavlink_msg_raw_rpm_decode(msg, &rpm); + rpm_s rpmmsg{}; + rpmmsg.timestamp = hrt_absolute_time(); + rpmmsg.indicated_frequency_rpm = rpm.frequency; + rpmmsg.estimated_accurancy_rpm = 0; + + _rpm_pub.publish(rpmmsg); + break; } }