diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 6807f56a43..6134a33452 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -112,6 +112,7 @@ set(config_module_list modules/systemlib/param modules/systemlib modules/uORB + modules/landing_target_estimator # # Libraries diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 7dcac0d9d3..ca116e8399 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -87,7 +87,7 @@ ExternalProject_Add_Step(sitl_gazebo forceconfigure # create targets for each viewer/model/debugger combination set(viewers none jmavsim gazebo replay) 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) foreach(viewer ${viewers}) foreach(debugger ${debuggers}) diff --git a/posix-configs/SITL/init/ekf2/iris_irlock b/posix-configs/SITL/init/ekf2/iris_irlock new file mode 100644 index 0000000000..c3b9eceba2 --- /dev/null +++ b/posix-configs/SITL/init/ekf2/iris_irlock @@ -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 diff --git a/posix-configs/SITL/init/lpe/iris_irlock b/posix-configs/SITL/init/lpe/iris_irlock new file mode 100644 index 0000000000..abc1c0ed5a --- /dev/null +++ b/posix-configs/SITL/init/lpe/iris_irlock @@ -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 diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index c87b7dace6..96fd11d71a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -335,6 +336,7 @@ private: orb_advert_t _vision_attitude_pub; orb_advert_t _dist_pub; orb_advert_t _battery_pub; + orb_advert_t _irlock_report_pub; int _param_sub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c641ec5db2..6f2e575c38 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -429,6 +429,25 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) 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: mavlink_hil_state_quaternion_t hil_state; mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);