diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 55c5daa263314a1db25be7de2ccfee3fb4e74998..0b6650ac2b9f17db06b0118ec875810c973cee56 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -30,7 +30,12 @@ pipeline { ] def nuttx_builds_archive = [ - target: ["px4_fmu-v2_default", "px4_fmu-v3_default", "px4_fmu-v4_default", "px4_fmu-v4pro_default", "px4_fmu-v5_default", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", + target: [ + "px4_fmu-v2_default", "px4_fmu-v2_fixedwing", "px4_fmu-v2_lpe", "px4_fmu-v2_multicopter", "px4_fmu-v2_rover", "px4_fmu-v2_test", + "px4_fmu-v3_default", + "px4_fmu-v4_default", + "px4_fmu-v4pro_default", + "px4_fmu-v5_default", "px4_fmu-v5_fixedwing", "px4_fmu-v5_multicopter", "px4_fmu-v5_rover", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", "intel_aerofc-v1_default", "gumstix_aerocore2_default", "auav_x21_default", "av_x-v1_default", "bitcraze_crazyflie_default", "airmind_mindpx-v2_default", "nxp_fmuk66-v3_default", "omnibus_f4sd_default"], image: docker_images.nuttx, diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake new file mode 100644 index 0000000000000000000000000000000000000000..162f82d955be47c8c64bd889ada0ca77c69eb6fc --- /dev/null +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -0,0 +1,83 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v2 + LABEL fixedwing + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + #TESTING + #UAVCAN_INTERFACES 2 + + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS6 + + DRIVERS + #barometer # all available barometer drivers + barometer/ms5611 + batt_smbus + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + gps + imu/l3gd20 + imu/lsm303d + imu/mpu6000 + imu/mpu9250 + lights/rgbled + #magnetometer # all available magnetometer drivers + magnetometer/hmc5883 + px4fmu + px4io + stm32 + stm32/adc + stm32/tone_alarm + #telemetry # all available telemetry drivers + telemetry/iridiumsbd + #uavcan + + MODULES + camera_feedback + commander + dataman + ekf2 + events + fw_att_control + fw_pos_control_l1 + land_detector + load_mon + logger + mavlink + navigator + sensors + vmount + wind_estimator + + SYSTEMCMDS + bl_update + #config + #dumpfile + #esc_calib + hardfault_log + #led_control + mixer + #motor_ramp + #motor_test + mtd + #nshterm + param + perf + pwm + reboot + #sd_bench + top + #topic_listener + tune_control + usb_connected + ver + ) diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake new file mode 100644 index 0000000000000000000000000000000000000000..2f5b4c505b6eb601dc4addc678f195e9c8f43db4 --- /dev/null +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -0,0 +1,80 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v2 + LABEL multicopter + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + #UAVCAN_INTERFACES 2 + + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS6 + + DRIVERS + barometer/ms5611 + batt_smbus + camera_trigger + distance_sensor # all available distance sensor drivers + gps + imu/l3gd20 + imu/lsm303d + imu/mpu6000 + imu/mpu9250 + irlock + lights/rgbled + magnetometer/hmc5883 + px4flow + px4fmu + px4io + stm32 + stm32/adc + stm32/tone_alarm + + MODULES + #attitude_estimator_q + camera_feedback + commander + dataman + ekf2 + events + land_detector + landing_target_estimator + load_mon + #local_position_estimator + logger + mavlink + mc_att_control + mc_pos_control + navigator + sensors + vmount + + SYSTEMCMDS + bl_update + #config + #dumpfile + #esc_calib + hardfault_log + #led_control + mixer + #motor_ramp + #motor_test + mtd + #nshterm + param + perf + pwm + reboot + #sd_bench + top + #topic_listener + tune_control + usb_connected + ver + ) diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake new file mode 100644 index 0000000000000000000000000000000000000000..bb42a84dff9bebed0733a97b7f014d8ae09589c3 --- /dev/null +++ b/boards/px4/fmu-v2/rover.cmake @@ -0,0 +1,75 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v2 + LABEL rover + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS6 + + DRIVERS + barometer/ms5611 + batt_smbus + camera_trigger + distance_sensor # all available distance sensor drivers + gps + imu/l3gd20 + imu/lsm303d + imu/mpu6000 + imu/mpu9250 + lights/rgbled + magnetometer/hmc5883 + px4flow + px4fmu + px4io + stm32 + stm32/adc + stm32/tone_alarm + + MODULES + camera_feedback + commander + dataman + ekf2 + events + gnd_att_control + gnd_pos_control + land_detector + load_mon + logger + mavlink + navigator + sensors + vmount + + SYSTEMCMDS + bl_update + #config + #dumpfile + #esc_calib + hardfault_log + #led_control + mixer + #motor_ramp + #motor_test + mtd + #nshterm + param + perf + pwm + reboot + #sd_bench + top + #topic_listener + tune_control + usb_connected + ver + ) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 862559a3670b16e411c4deb28f66da601426d1c6..3ee2532daa1d5ea172749a399a60d0e2c9f861ef 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -27,22 +27,20 @@ px4_add_board( #heater imu/adis16448 #imu # all available imu drivers - imu/bma180 imu/bmi055 - imu/bmi160 imu/mpu6000 imu/mpu9250 imu/icm20948 irlock lights/blinkm lights/oreoled + lights/pca8574 lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers #md25 mkblctrl - lights/pca8574 pca9685 pmw3901 #protocol_splitter diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake new file mode 100644 index 0000000000000000000000000000000000000000..008891fb10aa4439a931007a7b7fc718a032d748 --- /dev/null +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -0,0 +1,85 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v5 + LABEL fixedwing + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + UAVCAN_INTERFACES 2 + + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 + + DRIVERS + barometer # all available barometer drivers + batt_smbus + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + gps + imu/adis16448 + imu/bmi055 + imu/mpu6000 + lights/rgbled + lights/rgbled_ncp5623c + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + pwm_input + pwm_out_sim + px4fmu + px4io + rc_input + stm32 + stm32/adc + stm32/tone_alarm + telemetry # all available telemetry drivers + uavcan + + MODULES + camera_feedback + commander + dataman + ekf2 + events + fw_att_control + fw_pos_control_l1 + land_detector + load_mon + logger + mavlink + navigator + sensors + vmount + wind_estimator + + SYSTEMCMDS + bl_update + config + dumpfile + esc_calib + hardfault_log + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + shutdown + top + topic_listener + tune_control + usb_connected + ver + ) diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake new file mode 100644 index 0000000000000000000000000000000000000000..5ca751398806c7d48e7fb0d1a4a382f814bd4542 --- /dev/null +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -0,0 +1,94 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v5 + LABEL multicopter + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + TESTING + UAVCAN_INTERFACES 2 + + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 + + DRIVERS + barometer # all available barometer drivers + batt_smbus + camera_trigger + distance_sensor # all available distance sensor drivers + gps + imu/bmi055 + imu/mpu6000 + irlock + lights/blinkm + lights/oreoled + lights/rgbled + lights/rgbled_ncp5623c + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + pmw3901 + pwm_input + pwm_out_sim + px4flow + px4fmu + px4io + rc_input + roboclaw + stm32 + stm32/adc + stm32/tone_alarm + tap_esc + telemetry # all available telemetry drivers + uavcan + + MODULES + attitude_estimator_q + camera_feedback + commander + dataman + ekf2 + events + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_pos_control + navigator + sensors + vmount + wind_estimator + + SYSTEMCMDS + bl_update + config + dumpfile + esc_calib + hardfault_log + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + shutdown + top + topic_listener + tune_control + usb_connected + ver + ) diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake new file mode 100644 index 0000000000000000000000000000000000000000..5439f106cae392890edfb8fb98c146ed5cb1df43 --- /dev/null +++ b/boards/px4/fmu-v5/rover.cmake @@ -0,0 +1,89 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v5 + LABEL rover + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + UAVCAN_INTERFACES 2 + + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 + + DRIVERS + barometer # all available barometer drivers + batt_smbus + camera_trigger + distance_sensor # all available distance sensor drivers + gps + imu/bmi055 + imu/mpu6000 + lights/pca8574 + lights/rgbled + lights/rgbled_ncp5623c + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + #md25 + mkblctrl + pca9685 + pmw3901 + pwm_input + pwm_out_sim + px4flow + px4fmu + px4io + rc_input + roboclaw + stm32 + stm32/adc + stm32/tone_alarm + telemetry # all available telemetry drivers + uavcan + + MODULES + camera_feedback + commander + dataman + ekf2 + events + gnd_att_control + gnd_pos_control + land_detector + load_mon + logger + mavlink + navigator + sensors + vmount + + SYSTEMCMDS + bl_update + config + dumpfile + esc_calib + hardfault_log + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + shutdown + top + topic_listener + tune_control + usb_connected + ver + ) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index bb240ac93bcb76899083de6c87043fcc73259841..13ae2ae1905c00d7b81c4761e79510041d28faf8 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -63,6 +63,10 @@ PrecLand::PrecLand(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { + _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); + _handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE"); + + updateParams(); } void @@ -101,7 +105,6 @@ PrecLand::on_activation() _last_slewrate_time = 0; switch_to_state_start(); - } void @@ -160,6 +163,20 @@ PrecLand::on_active() } +void +PrecLand::updateParams() +{ + ModuleParams::updateParams(); + + if (_handle_param_acceleration_hor != PARAM_INVALID) { + param_get(_handle_param_acceleration_hor, &_param_acceleration_hor); + } + + if (_handle_param_xy_vel_cruise != PARAM_INVALID) { + param_get(_handle_param_xy_vel_cruise, &_param_xy_vel_cruise); + } +} + void PrecLand::run_state_start() { @@ -545,21 +562,21 @@ void PrecLand::slewrate(float &sp_x, float &sp_y) // limit the setpoint speed to the maximum cruise speed matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints - if (sp_vel.length() > _param_xy_vel_cruise.get()) { - sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get(); + if (sp_vel.length() > _param_xy_vel_cruise) { + sp_vel = sp_vel.normalized() * _param_xy_vel_cruise; sp_curr = _sp_pev + sp_vel * dt; } // limit the setpoint acceleration to the maximum acceleration matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints - if (sp_acc.length() > _param_acceleration_hor.get()) { - sp_acc = sp_acc.normalized() * _param_acceleration_hor.get(); + if (sp_acc.length() > _param_acceleration_hor) { + sp_acc = sp_acc.normalized() * _param_acceleration_hor; sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt); } // limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration - float max_spd = sqrtf(_param_acceleration_hor.get() * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x, + float max_spd = sqrtf(_param_acceleration_hor * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x, sp_y))).length()); sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index 44b68b3ea7d5a85576b846f3470ade8ae362b9ef..1bc205aa8f2c499afcbcda36381480888bebf4a8 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -77,6 +77,9 @@ public: PrecLandMode get_mode() { return _mode; }; private: + + void updateParams() override; + // run the control loop for each state void run_state_start(); void run_state_horizontal_approach(); @@ -127,9 +130,13 @@ private: (ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_final_approach_alt, (ParamFloat<px4::params::PLD_SRCH_ALT>) _param_search_alt, (ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_search_timeout, - (ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches, - (ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor, - (ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise + (ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches ) + // non-navigator parameters + param_t _handle_param_acceleration_hor{PARAM_INVALID}; + param_t _handle_param_xy_vel_cruise{PARAM_INVALID}; + float _param_acceleration_hor{0.0f}; + float _param_xy_vel_cruise{0.0f}; + };