yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add support for testing precision landing in SITL simulation

Browse Source
This commit is contained in:
Nicolas de Palezieux
2018-01-14 23:21:23 +05:30
committed by Lorenz Meier
parent d9221bb8d2
commit f600cfbb9f
6 changed files with 191 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
cmake/configs/posix_sitl_default.cmake
View File

@@ -112,6 +112,7 @@ set(config_module_list
modules/systemlib/param modules/systemlib/param
modules/systemlib modules/systemlib
modules/uORB modules/uORB
modules/landing_target_estimator
# #
# Libraries # Libraries

2
platforms/posix/cmake/sitl_target.cmake
View File

@@ -87,7 +87,7 @@ ExternalProject_Add_Step(sitl_gazebo forceconfigure
# create targets for each viewer/model/debugger combination # create targets for each viewer/model/debugger combination
set(viewers none jmavsim gazebo replay) set(viewers none jmavsim gazebo replay)
set(debuggers none ide gdb lldb ddd valgrind callgrind) set(debuggers none ide gdb lldb ddd valgrind callgrind)
set(models none iris iris_opt_flow iris_rplidar standard_vtol plane solo tailsitter typhoon_h480 rover hippocampus) set(models none iris iris_opt_flow iris_rplidar iris_irlock standard_vtol plane solo tailsitter typhoon_h480 rover hippocampus)
set(all_posix_vmd_make_targets) set(all_posix_vmd_make_targets)
foreach(viewer ${viewers}) foreach(viewer ${viewers})
foreach(debugger ${debuggers}) foreach(debugger ${debuggers})

80
posix-configs/SITL/init/ekf2/iris_irlock Normal file
View File

@@ -0,0 +1,80 @@
uorb start
param load
dataman start
param set BAT_N_CELLS 3
param set CAL_ACC0_ID 1376264
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_ID 1310728
param set CAL_ACC1_XOFF 0.01
param set CAL_GYRO0_ID 2293768
param set CAL_GYRO0_XOFF 0.01
param set CAL_MAG0_ID 196616
param set CAL_MAG0_XOFF 0.01
param set COM_DISARM_LAND 3
param set COM_OBL_ACT 2
param set COM_OBL_RC_ACT 0
param set COM_OF_LOSS_T 5
param set COM_RC_IN_MODE 1
param set EKF2_AID_MASK 1
param set EKF2_ANGERR_INIT 0.01
param set EKF2_GBIAS_INIT 0.01
param set EKF2_HGT_MODE 0
param set EKF2_MAG_TYPE 1
param set MAV_TYPE 2
param set MC_PITCH_P 6
param set MC_PITCHRATE_P 0.2
param set MC_ROLL_P 6
param set MC_ROLLRATE_P 0.2
param set MIS_TAKEOFF_ALT 2.5
param set MPC_HOLD_MAX_Z 2.0
param set MPC_Z_VEL_I 0.15
param set MPC_Z_VEL_P 0.6
param set NAV_ACC_RAD 2.0
param set NAV_DLL_ACT 2
param set RTL_DESCEND_ALT 5.0
param set RTL_LAND_DELAY 5
param set RTL_RETURN_ALT 30.0
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set SYS_AUTOSTART 4010
param set SYS_MC_EST_GROUP 2
param set SYS_RESTART_TYPE 2
replay tryapplyparams
simulator start -s
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sensors start
commander start
land_detector start multicopter
navigator start
ekf2 start
landing_target_estimator start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_dc.main.mix
mavlink start -u 14556 -r 4000000
mavlink start -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 50 -s ATTITUDE -u 14556
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
logger start -e -t
mavlink boot_complete
replay trystart

88
posix-configs/SITL/init/lpe/iris_irlock Normal file
View File

@@ -0,0 +1,88 @@
uorb start
param load
param set MAV_TYPE 2
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
param set SYS_MC_EST_GROUP 1
dataman start
param set BAT_N_CELLS 3
param set CAL_GYRO0_ID 2293768
param set CAL_ACC0_ID 1376264
param set CAL_ACC1_ID 1310728
param set CAL_MAG0_ID 196616
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set COM_DISARM_LAND 3
param set NAV_ACC_RAD 2.0
param set COM_OF_LOSS_T 5
param set COM_OBL_ACT 2
param set COM_OBL_RC_ACT 0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 5.0
param set RTL_LAND_DELAY 5
param set MIS_TAKEOFF_ALT 2.5
param set MC_ROLLRATE_P 0.2
param set MC_PITCHRATE_P 0.2
param set MC_PITCH_P 6
param set MC_ROLL_P 6
param set MPC_HOLD_MAX_Z 2.0
param set MPC_Z_VEL_P 0.6
param set MPC_Z_VEL_I 0.15
param set MPC_XY_VEL_P 0.13
param set MPC_XY_P 1.2
param set MC_PITCHRATE_MAX 150
param set MC_ROLLRATE_MAX 150
param set EKF2_GBIAS_INIT 0.01
param set EKF2_ANGERR_INIT 0.01
# GPS only mode
param set LPE_FUSION 145
param set LPE_EPH_MAX 5
# enable fusion of landing target velocity
param set LTEST_MODE 1
param set PLD_HACC_RAD 0.1
replay tryapplyparams
simulator start -s
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
local_position_estimator start
landing_target_estimator start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_dc.main.mix
mavlink start -u 14556 -r 4000000
mavlink start -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 50 -s ATTITUDE -u 14556
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
logger start -e -t
mavlink boot_complete
replay trystart

2
src/modules/simulator/simulator.h
View File

@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h> #include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h> #include <drivers/drv_gyro.h>
@@ -335,6 +336,7 @@ private:
orb_advert_t _vision_attitude_pub; orb_advert_t _vision_attitude_pub;
orb_advert_t _dist_pub; orb_advert_t _dist_pub;
orb_advert_t _battery_pub; orb_advert_t _battery_pub;
orb_advert_t _irlock_report_pub;
int _param_sub; int _param_sub;

19
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -429,6 +429,25 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
break; break;
case MAVLINK_MSG_ID_LANDING_TARGET:
mavlink_landing_target_t landing_target_mavlink;
mavlink_msg_landing_target_decode(msg, &landing_target_mavlink);
struct irlock_report_s report;
memset(&report, 0, sizeof(report));
report.timestamp = hrt_absolute_time();
report.signature = landing_target_mavlink.target_num;
report.pos_x = landing_target_mavlink.angle_x;
report.pos_y = landing_target_mavlink.angle_y;
report.size_x = landing_target_mavlink.size_x;
report.size_y = landing_target_mavlink.size_y;
int irlock_multi;
orb_publish_auto(ORB_ID(irlock_report), &_irlock_report_pub, &report, &irlock_multi, ORB_PRIO_HIGH);
break;
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
mavlink_hil_state_quaternion_t hil_state; mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);