mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
create example vehicle type build configs for fmu-v2 and fmu-v5 (#10963)
- update navigator precision landing to build without multicopter
This commit is contained in:
@@ -30,7 +30,12 @@ pipeline {
|
|||||||
]
|
]
|
||||||
|
|
||||||
def nuttx_builds_archive = [
|
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",
|
"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"],
|
"nxp_fmuk66-v3_default", "omnibus_f4sd_default"],
|
||||||
image: docker_images.nuttx,
|
image: docker_images.nuttx,
|
||||||
|
|||||||
83
boards/px4/fmu-v2/fixedwing.cmake
Normal file
83
boards/px4/fmu-v2/fixedwing.cmake
Normal file
@@ -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
|
||||||
|
)
|
||||||
80
boards/px4/fmu-v2/multicopter.cmake
Normal file
80
boards/px4/fmu-v2/multicopter.cmake
Normal file
@@ -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
|
||||||
|
)
|
||||||
75
boards/px4/fmu-v2/rover.cmake
Normal file
75
boards/px4/fmu-v2/rover.cmake
Normal file
@@ -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
|
||||||
|
)
|
||||||
@@ -27,22 +27,20 @@ px4_add_board(
|
|||||||
#heater
|
#heater
|
||||||
imu/adis16448
|
imu/adis16448
|
||||||
#imu # all available imu drivers
|
#imu # all available imu drivers
|
||||||
imu/bma180
|
|
||||||
imu/bmi055
|
imu/bmi055
|
||||||
imu/bmi160
|
|
||||||
imu/mpu6000
|
imu/mpu6000
|
||||||
imu/mpu9250
|
imu/mpu9250
|
||||||
imu/icm20948
|
imu/icm20948
|
||||||
irlock
|
irlock
|
||||||
lights/blinkm
|
lights/blinkm
|
||||||
lights/oreoled
|
lights/oreoled
|
||||||
|
lights/pca8574
|
||||||
lights/rgbled
|
lights/rgbled
|
||||||
lights/rgbled_ncp5623c
|
lights/rgbled_ncp5623c
|
||||||
lights/rgbled_pwm
|
lights/rgbled_pwm
|
||||||
magnetometer # all available magnetometer drivers
|
magnetometer # all available magnetometer drivers
|
||||||
#md25
|
#md25
|
||||||
mkblctrl
|
mkblctrl
|
||||||
lights/pca8574
|
|
||||||
pca9685
|
pca9685
|
||||||
pmw3901
|
pmw3901
|
||||||
#protocol_splitter
|
#protocol_splitter
|
||||||
|
|||||||
85
boards/px4/fmu-v5/fixedwing.cmake
Normal file
85
boards/px4/fmu-v5/fixedwing.cmake
Normal file
@@ -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
|
||||||
|
)
|
||||||
94
boards/px4/fmu-v5/multicopter.cmake
Normal file
94
boards/px4/fmu-v5/multicopter.cmake
Normal file
@@ -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
|
||||||
|
)
|
||||||
89
boards/px4/fmu-v5/rover.cmake
Normal file
89
boards/px4/fmu-v5/rover.cmake
Normal file
@@ -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
|
||||||
|
)
|
||||||
@@ -63,6 +63,10 @@ PrecLand::PrecLand(Navigator *navigator) :
|
|||||||
MissionBlock(navigator),
|
MissionBlock(navigator),
|
||||||
ModuleParams(navigator)
|
ModuleParams(navigator)
|
||||||
{
|
{
|
||||||
|
_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
|
||||||
|
_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
|
||||||
|
|
||||||
|
updateParams();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
@@ -101,7 +105,6 @@ PrecLand::on_activation()
|
|||||||
_last_slewrate_time = 0;
|
_last_slewrate_time = 0;
|
||||||
|
|
||||||
switch_to_state_start();
|
switch_to_state_start();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
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
|
void
|
||||||
PrecLand::run_state_start()
|
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
|
// limit the setpoint speed to the maximum cruise speed
|
||||||
matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
|
matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
|
||||||
|
|
||||||
if (sp_vel.length() > _param_xy_vel_cruise.get()) {
|
if (sp_vel.length() > _param_xy_vel_cruise) {
|
||||||
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get();
|
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise;
|
||||||
sp_curr = _sp_pev + sp_vel * dt;
|
sp_curr = _sp_pev + sp_vel * dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
// limit the setpoint acceleration to the maximum acceleration
|
// 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
|
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()) {
|
if (sp_acc.length() > _param_acceleration_hor) {
|
||||||
sp_acc = sp_acc.normalized() * _param_acceleration_hor.get();
|
sp_acc = sp_acc.normalized() * _param_acceleration_hor;
|
||||||
sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt);
|
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
|
// 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_y))).length());
|
||||||
sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
|
sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
|
||||||
|
|
||||||
|
|||||||
@@ -77,6 +77,9 @@ public:
|
|||||||
PrecLandMode get_mode() { return _mode; };
|
PrecLandMode get_mode() { return _mode; };
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
void updateParams() override;
|
||||||
|
|
||||||
// run the control loop for each state
|
// run the control loop for each state
|
||||||
void run_state_start();
|
void run_state_start();
|
||||||
void run_state_horizontal_approach();
|
void run_state_horizontal_approach();
|
||||||
@@ -127,9 +130,13 @@ private:
|
|||||||
(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_final_approach_alt,
|
(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_final_approach_alt,
|
||||||
(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_search_alt,
|
(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_search_alt,
|
||||||
(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_search_timeout,
|
(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_search_timeout,
|
||||||
(ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches,
|
(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
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
// 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};
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user