mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-11 14:40:06 +08:00
Compare commits
30 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 3d7f6ea09c | |||
| d259386987 | |||
| 14c4257a59 | |||
| b71c6fb6ea | |||
| adb22f1407 | |||
| 0c97b0f4b0 | |||
| 5ca22df55c | |||
| a3515a2474 | |||
| 6bd1145006 | |||
| 1df52df27d | |||
| 97423136d1 | |||
| c5101c70b3 | |||
| df46ad7774 | |||
| d1b8a2e8d5 | |||
| ae3aee3402 | |||
| 1d07697a9e | |||
| 1c25d65a1e | |||
| 4c0b6dbe86 | |||
| d8d2213cab | |||
| 9184a8f4ef | |||
| 3ceb932b7c | |||
| 938be68c69 | |||
| b0df7c7ccb | |||
| 789b3880cf | |||
| 0d8ba587ca | |||
| 178ea132b6 | |||
| fe7988672f | |||
| aaefc36cad | |||
| 6c9af2e0ec | |||
| 6b4fca1b9d |
@@ -52,12 +52,8 @@ param set-default MAV_1_MODE 9
|
||||
# param set-default SER_TEL1_BAUD 921600 Not found
|
||||
|
||||
# Vehicle attitude PID tuning
|
||||
param set-default MC_ACRO_EXPO 0
|
||||
param set-default MC_ACRO_EXPO_Y 0
|
||||
param set-default MC_ACRO_P_MAX 200
|
||||
param set-default MC_ACRO_R_MAX 200
|
||||
param set-default MC_ACRO_SUPEXPO 0
|
||||
param set-default MC_ACRO_SUPEXPOY 0
|
||||
param set-default MC_ACRO_Y_MAX 150
|
||||
param set-default MC_PITCHRATE_D 0.0015
|
||||
param set-default MC_ROLLRATE_D 0.0015
|
||||
|
||||
@@ -143,12 +143,9 @@ then
|
||||
param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
|
||||
|
||||
param set CAL_MAG0_ID 197388
|
||||
param set SENS_MAG0_ID 197388
|
||||
param set SENS_MAG0_PRIO 50
|
||||
|
||||
param set CAL_MAG0_PRIO 50
|
||||
param set CAL_MAG1_ID 197644
|
||||
param set SENS_MAG1_ID 197644
|
||||
param set SENS_MAG1_PRIO 50
|
||||
param set CAL_MAG1_PRIO 50
|
||||
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SENS_DPRES_OFF 0.001
|
||||
|
||||
@@ -31,7 +31,7 @@ param set-default BAT1_R_INTERNAL 0.0025
|
||||
param set-default CBRK_AIRSPD_CHK 162128
|
||||
param set-default CBRK_IO_SAFETY 22027
|
||||
|
||||
param set-default SENS_GNSS0_XPOS -0.12
|
||||
param set-default EKF2_GPS_POS_X -0.12
|
||||
param set-default EKF2_IMU_POS_X -0.12
|
||||
|
||||
param set-default FW_ARSP_MODE 1
|
||||
|
||||
@@ -18,8 +18,8 @@ param set-default COM_DISARM_LAND 0.5
|
||||
# EKF2 parameters
|
||||
param set-default EKF2_DRAG_CTRL 1
|
||||
param set-default EKF2_IMU_POS_X 0.02
|
||||
param set-default SENS_GNSS0_XPOS 0.055
|
||||
param set-default SENS_GNSS0_ZPOS -0.15
|
||||
param set-default EKF2_GPS_POS_X 0.055
|
||||
param set-default EKF2_GPS_POS_Z -0.15
|
||||
param set-default EKF2_MIN_RNG 0.03
|
||||
param set-default EKF2_OF_CTRL 1
|
||||
param set-default EKF2_OF_POS_X 0.055
|
||||
@@ -45,12 +45,8 @@ param set-default MAV_1_MODE 9
|
||||
param set-default SER_TEL1_BAUD 921600
|
||||
|
||||
# Vehicle attitude PID tuning
|
||||
param set-default MC_ACRO_EXPO 0
|
||||
param set-default MC_ACRO_EXPO_Y 0
|
||||
param set-default MC_ACRO_P_MAX 200
|
||||
param set-default MC_ACRO_R_MAX 200
|
||||
param set-default MC_ACRO_SUPEXPO 0
|
||||
param set-default MC_ACRO_SUPEXPOY 0
|
||||
param set-default MC_ACRO_Y_MAX 150
|
||||
param set-default MC_PITCHRATE_D 0.0015
|
||||
param set-default MC_ROLLRATE_D 0.0015
|
||||
|
||||
@@ -45,12 +45,8 @@ param set-default MAV_1_MODE 9
|
||||
param set-default SER_TEL1_BAUD 921600
|
||||
|
||||
# Vehicle attitude PID tuning
|
||||
param set-default MC_ACRO_EXPO 0
|
||||
param set-default MC_ACRO_EXPO_Y 0
|
||||
param set-default MC_ACRO_P_MAX 200
|
||||
param set-default MC_ACRO_R_MAX 200
|
||||
param set-default MC_ACRO_SUPEXPO 0
|
||||
param set-default MC_ACRO_SUPEXPOY 0
|
||||
param set-default MC_ACRO_Y_MAX 150
|
||||
param set-default MC_PITCHRATE_D 0.0015
|
||||
param set-default MC_ROLLRATE_D 0.0015
|
||||
|
||||
@@ -1,65 +0,0 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name Reaper 500 Quad
|
||||
#
|
||||
# @type Quadrotor H
|
||||
# @class Copter
|
||||
#
|
||||
# @maintainer Blankered
|
||||
#
|
||||
# @board px4_fmu-v2 exclude
|
||||
# @board bitcraze_crazyflie exclude
|
||||
#
|
||||
|
||||
. ${R}etc/init.d/rc.mc_defaults
|
||||
|
||||
param set-default MC_ROLLRATE_P 0.14
|
||||
param set-default MC_ROLLRATE_I 0.1
|
||||
param set-default MC_ROLLRATE_D 0.004
|
||||
param set-default MC_PITCH_P 6
|
||||
param set-default MC_PITCHRATE_P 0.14
|
||||
param set-default MC_PITCHRATE_I 0.09
|
||||
param set-default MC_PITCHRATE_D 0.004
|
||||
param set-default MC_YAW_P 4
|
||||
|
||||
param set-default NAV_ACC_RAD 2
|
||||
|
||||
param set-default RTL_RETURN_ALT 30
|
||||
param set-default RTL_DESCEND_ALT 10
|
||||
|
||||
param set-default CA_ROTOR_COUNT 4
|
||||
param set-default CA_ROTOR0_PX 0.15
|
||||
param set-default CA_ROTOR0_PY 0.15
|
||||
param set-default CA_ROTOR0_KM -0.05
|
||||
param set-default CA_ROTOR1_PX -0.15
|
||||
param set-default CA_ROTOR1_PY -0.15
|
||||
param set-default CA_ROTOR1_KM -0.05
|
||||
param set-default CA_ROTOR2_PX 0.15
|
||||
param set-default CA_ROTOR2_PY -0.15
|
||||
param set-default CA_ROTOR2_KM 0.05
|
||||
param set-default CA_ROTOR3_PX -0.15
|
||||
param set-default CA_ROTOR3_PY 0.15
|
||||
param set-default CA_ROTOR3_KM 0.05
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
param set-default PWM_MAIN_MIN1 1100
|
||||
param set-default PWM_MAIN_MIN2 1100
|
||||
param set-default PWM_MAIN_MIN3 1100
|
||||
param set-default PWM_MAIN_MIN4 1100
|
||||
param set-default PWM_MAIN_MAX1 1900
|
||||
param set-default PWM_MAIN_MAX2 1900
|
||||
param set-default PWM_MAIN_MAX3 1900
|
||||
param set-default PWM_MAIN_MAX4 1900
|
||||
|
||||
param set-default PWM_MAIN_TIM0 50
|
||||
param set-default PWM_MAIN_TIM1 50
|
||||
param set-default PWM_MAIN_TIM2 50
|
||||
|
||||
param set-default PWM_AUX_TIM0 50
|
||||
param set-default PWM_AUX_TIM1 50
|
||||
param set-default PWM_AUX_TIM2 50
|
||||
|
||||
|
||||
@@ -46,9 +46,7 @@ param set-default EKF2_BCOEF_Y 25.5
|
||||
param set-default EKF2_DRAG_CTRL 1
|
||||
|
||||
param set-default EKF2_GPS_DELAY 100
|
||||
param set-default SENS_GNSS0_XPOS 0.06
|
||||
param set-default SENS_GNSS0_YPOS 0.0
|
||||
param set-default SENS_GNSS0_ZPOS 0.0
|
||||
param set-default EKF2_GPS_POS_X 0.06
|
||||
param set-default EKF2_GPS_V_NOISE 0.5
|
||||
|
||||
param set-default EKF2_IMU_POS_X 0.06
|
||||
|
||||
@@ -1,91 +0,0 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name Crazyflie 2
|
||||
#
|
||||
# @type Quadrotor x
|
||||
# @class Copter
|
||||
#
|
||||
# @maintainer Dennis Shtatov <densht@gmail.com>
|
||||
#
|
||||
# @board px4_fmu-v2 exclude
|
||||
# @board px4_fmu-v3 exclude
|
||||
# @board px4_fmu-v4 exclude
|
||||
# @board px4_fmu-v4pro exclude
|
||||
# @board px4_fmu-v5 exclude
|
||||
# @board px4_fmu-v5x exclude
|
||||
# @board diatone_mamba-f405-mk2 exclude
|
||||
#
|
||||
. ${R}etc/init.d/rc.mc_defaults
|
||||
|
||||
param set-default BAT1_N_CELLS 1
|
||||
param set-default BAT1_CAPACITY 240
|
||||
param set-default BAT1_SOURCE 1
|
||||
|
||||
param set-default CBRK_SUPPLY_CHK 894281
|
||||
param set-default COM_RC_IN_MODE 1
|
||||
|
||||
param set-default EKF2_ABL_LIM 2
|
||||
param set-default EKF2_HGT_REF 2
|
||||
param set-default EKF2_RNG_CTRL 2
|
||||
param set-default EKF2_MAG_TYPE 1
|
||||
param set-default EKF2_OF_CTRL 1
|
||||
param set-default EKF2_OF_DELAY 10
|
||||
|
||||
param set-default IMU_GYRO_CUTOFF 100
|
||||
param set-default IMU_ACCEL_CUTOFF 30
|
||||
|
||||
param set-default MC_AIRMODE 1
|
||||
param set-default IMU_DGYRO_CUTOFF 70
|
||||
param set-default MC_PITCHRATE_D 0.002
|
||||
param set-default MC_PITCHRATE_P 0.07
|
||||
param set-default MC_ROLLRATE_D 0.002
|
||||
param set-default MC_ROLLRATE_P 0.07
|
||||
param set-default MC_YAW_P 3
|
||||
|
||||
param set-default MPC_THR_HOVER 0.7
|
||||
param set-default MPC_Z_P 1.5
|
||||
param set-default MPC_Z_VEL_P_ACC 8
|
||||
param set-default MPC_Z_VEL_I_ACC 6
|
||||
param set-default MPC_HOLD_MAX_XY 0.1
|
||||
param set-default MPC_MAX_FLOW_HGT 3
|
||||
|
||||
param set-default NAV_RCL_ACT 3
|
||||
|
||||
param set-default CA_ROTOR_COUNT 4
|
||||
param set-default CA_ROTOR0_PX 0.03
|
||||
param set-default CA_ROTOR0_PY 0.03
|
||||
param set-default CA_ROTOR1_PX -0.03
|
||||
param set-default CA_ROTOR1_PY 0.03
|
||||
param set-default CA_ROTOR1_KM -0.05
|
||||
param set-default CA_ROTOR2_PX -0.03
|
||||
param set-default CA_ROTOR2_PY -0.03
|
||||
param set-default CA_ROTOR3_PX 0.03
|
||||
param set-default CA_ROTOR3_PY -0.03
|
||||
param set-default CA_ROTOR3_KM -0.05
|
||||
|
||||
# Run the motors at 328.125 kHz (recommended)
|
||||
param set-default PWM_MAIN_TIM0 3921
|
||||
param set-default PWM_MAIN_TIM1 3921
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
param set-default PWM_MAIN_DIS0 0
|
||||
param set-default PWM_MAIN_DIS1 0
|
||||
param set-default PWM_MAIN_DIS2 0
|
||||
param set-default PWM_MAIN_DIS3 0
|
||||
param set-default PWM_MAIN_MIN0 0
|
||||
param set-default PWM_MAIN_MIN1 0
|
||||
param set-default PWM_MAIN_MIN2 0
|
||||
param set-default PWM_MAIN_MIN3 0
|
||||
param set-default PWM_MAIN_MAX0 255
|
||||
param set-default PWM_MAIN_MAX1 255
|
||||
param set-default PWM_MAIN_MAX2 255
|
||||
param set-default PWM_MAIN_MAX3 255
|
||||
|
||||
|
||||
param set-default SENS_FLOW_MINRNG 0.05
|
||||
|
||||
syslink start
|
||||
mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
|
||||
@@ -58,7 +58,6 @@ px4_add_romfs_files(
|
||||
4017_nxp_hovergames
|
||||
4019_x500_v2
|
||||
4020_holybro_px4vision_v1_5
|
||||
4040_reaper
|
||||
4041_beta75x
|
||||
4050_generic_250
|
||||
4052_holybro_qav250
|
||||
@@ -67,7 +66,6 @@ px4_add_romfs_files(
|
||||
4071_ifo
|
||||
4073_ifo-s
|
||||
4500_clover4
|
||||
4900_crazyflie
|
||||
4901_crazyflie21
|
||||
|
||||
# [5000, 5999] Quadrotor +"
|
||||
|
||||
@@ -17,7 +17,7 @@ param set-default COM_POS_FS_DELAY 5
|
||||
|
||||
# there is a 2.5 factor applied on the _FS thresholds if for invalidation
|
||||
param set-default COM_POS_FS_EPH 50
|
||||
param set-default COM_VEL_FS_EVH 5
|
||||
param set-default COM_VEL_FS_EVH 3
|
||||
|
||||
param set-default COM_POS_LOW_EPH 50
|
||||
|
||||
|
||||
@@ -302,12 +302,6 @@ When set to -1 (default), the value depends on the function (see {:}).
|
||||
for key, label, param_suffix, description in standard_params_array:
|
||||
if key in standard_params:
|
||||
|
||||
# values must be in range of an uint16_t
|
||||
if standard_params[key]['min'] < 0:
|
||||
raise Exception('minimum value for {:} expected >= 0 (got {:})'.format(key, standard_params[key]['min']))
|
||||
if standard_params[key]['max'] >= 1<<16:
|
||||
raise Exception('maximum value for {:} expected <= {:} (got {:})'.format(key, 1<<16, standard_params[key]['max']))
|
||||
|
||||
if key == 'failsafe':
|
||||
standard_params[key]['default'] = -1
|
||||
standard_params[key]['min'] = -1
|
||||
@@ -317,7 +311,7 @@ When set to -1 (default), the value depends on the function (see {:}).
|
||||
'short': channel_label+' ${i} '+label+' Value',
|
||||
'long': description
|
||||
},
|
||||
'type': 'int32',
|
||||
'type': 'float',
|
||||
'instance_start': instance_start,
|
||||
'instance_start_label': instance_start_label,
|
||||
'num_instances': num_channels,
|
||||
|
||||
@@ -146,6 +146,7 @@ def write_fields_to_hpp_file(file_name: str, definitions: dict, window_length: i
|
||||
format_list: [str]):
|
||||
max_tokenized_field_length, max_tokenized_field_length_msg = max(
|
||||
((len(definitions[k]['fields']), k) for k in definitions), key=itemgetter(0))
|
||||
max_untokenized_field_length = max(definitions[k]['fields_total_length'] for k in definitions)
|
||||
max_num_orb_ids = max(len(definitions[k]['orb_ids']) for k in definitions)
|
||||
max_num_orb_id_dependencies = max(len(definitions[k]['dependencies']) for k in definitions)
|
||||
|
||||
@@ -167,6 +168,7 @@ const uint8_t* orb_compressed_message_formats();
|
||||
unsigned orb_compressed_message_formats_size();
|
||||
|
||||
static constexpr unsigned orb_tokenized_fields_max_length = {MAX_TOKENIZED_FIELD_LENGTH}; // {MAX_TOKENIZED_FIELD_LENGTH_MSG}
|
||||
static constexpr unsigned orb_untokenized_fields_max_length = {MAX_UNTOKENIZED_FIELD_LENGTH};
|
||||
static constexpr unsigned orb_compressed_max_num_orb_ids = {MAX_NUM_ORB_IDS};
|
||||
static constexpr unsigned orb_compressed_max_num_orb_id_dependencies = {MAX_NUM_ORB_ID_DEPENDENCIES};
|
||||
|
||||
@@ -179,6 +181,7 @@ static constexpr unsigned orb_compressed_heatshrink_lookahead_length = {LOOKAHEA
|
||||
'''
|
||||
.replace('{MAX_TOKENIZED_FIELD_LENGTH}', str(max_tokenized_field_length))
|
||||
.replace('{MAX_TOKENIZED_FIELD_LENGTH_MSG}', max_tokenized_field_length_msg)
|
||||
.replace('{MAX_UNTOKENIZED_FIELD_LENGTH}', str(max_untokenized_field_length))
|
||||
.replace('{MAX_NUM_ORB_IDS}', str(max_num_orb_ids))
|
||||
.replace('{MAX_NUM_ORB_ID_DEPENDENCIES}', str(max_num_orb_id_dependencies))
|
||||
.replace('{WINDOW_LENGTH}', str(window_length))
|
||||
|
||||
@@ -42,6 +42,7 @@ for field in spec.parsed_fields():
|
||||
{
|
||||
@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed;"
|
||||
"fields": @( json.dumps(bytearray(";".join(topic_fields)+";", 'utf-8').decode('unicode_escape')) ),
|
||||
"fields_total_length": @(sum([len(convert_type(field.type))+1+len(field.name)+1 for field in sorted_fields])),
|
||||
"orb_ids": @( json.dumps([ all_topics.index(topic) for topic in topics]) ),
|
||||
"main_orb_id": @( all_topics.index(name_snake_case) if name_snake_case in all_topics else -1 ),
|
||||
"dependencies": @( json.dumps(list(set(dependencies))) ),
|
||||
|
||||
+10
-38
@@ -1,7 +1,7 @@
|
||||
#! /usr/bin/env bash
|
||||
|
||||
## Bash script to setup PX4 development environment on Arch Linux.
|
||||
## Tested on Manjaro 20.2.1.
|
||||
## Tested on Arch 2023-03-01
|
||||
##
|
||||
## Installs:
|
||||
## - Common dependencies and tools for nuttx, jMAVSim
|
||||
@@ -50,6 +50,7 @@ sudo pacman -Sy --noconfirm --needed \
|
||||
cmake \
|
||||
cppcheck \
|
||||
doxygen \
|
||||
fuse2 \
|
||||
gdb \
|
||||
git \
|
||||
gnutls \
|
||||
@@ -66,7 +67,7 @@ sudo pacman -Sy --noconfirm --needed \
|
||||
|
||||
# Python dependencies
|
||||
echo "Installing PX4 Python3 dependencies"
|
||||
pip install --user -r ${DIR}/requirements.txt
|
||||
pip install --break-system-packages -r ${DIR}/${REQUIREMENTS_FILE}
|
||||
|
||||
# NuttX toolchain (arm-none-eabi-gcc)
|
||||
if [[ $INSTALL_NUTTX == "true" ]]; then
|
||||
@@ -74,45 +75,17 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
|
||||
echo "Installing NuttX dependencies"
|
||||
|
||||
sudo pacman -S --noconfirm --needed \
|
||||
gperf \
|
||||
vim \
|
||||
arm-none-eabi-gcc \
|
||||
arm-none-eabi-newlib \
|
||||
;
|
||||
|
||||
if [ ! -z "$USER" ]; then
|
||||
# add user to dialout group (serial port access)
|
||||
sudo usermod -aG uucp $USER
|
||||
sudo echo usermod -aG uucp $USER
|
||||
fi
|
||||
|
||||
# remove modem manager (interferes with PX4 serial port usage)
|
||||
sudo pacman -R modemmanager --noconfirm
|
||||
|
||||
# arm-none-eabi-gcc
|
||||
NUTTX_GCC_VERSION="10-2020-q4-major"
|
||||
NUTTX_GCC_VERSION_SHORT="10-2020q4"
|
||||
|
||||
source $HOME/.profile # load changed path for the case the script is reran before relogin
|
||||
if [ $(which arm-none-eabi-gcc) ]; then
|
||||
GCC_VER_STR=$(arm-none-eabi-gcc --version)
|
||||
GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}")
|
||||
fi
|
||||
|
||||
if [[ "$GCC_FOUND_VER" == "1" ]]; then
|
||||
echo "arm-none-eabi-gcc-${NUTTX_GCC_VERSION} found, skipping installation"
|
||||
|
||||
else
|
||||
echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
|
||||
wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-x86_64-linux.tar.bz2 && \
|
||||
sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/;
|
||||
|
||||
# add arm-none-eabi-gcc to user's PATH
|
||||
exportline="export PATH=/opt/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}/bin:\$PATH"
|
||||
|
||||
if grep -Fxq "$exportline" $HOME/.profile; then
|
||||
echo "${NUTTX_GCC_VERSION} path already set.";
|
||||
else
|
||||
echo $exportline >> $HOME/.profile;
|
||||
fi
|
||||
fi
|
||||
# don't run modem manager (interferes with PX4 serial port usage)
|
||||
sudo systemctl disable --now ModemManager
|
||||
fi
|
||||
|
||||
# Simulation tools
|
||||
@@ -122,8 +95,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
|
||||
|
||||
# java (jmavsim)
|
||||
sudo pacman -S --noconfirm --needed \
|
||||
ant \
|
||||
jdk-openjdk \
|
||||
ant
|
||||
;
|
||||
|
||||
# Gazebo setup
|
||||
@@ -161,5 +133,5 @@ fi
|
||||
|
||||
if [[ $INSTALL_NUTTX == "true" ]]; then
|
||||
echo
|
||||
echo "Reboot or logout, login computer before attempting to build NuttX targets"
|
||||
echo "Reboot or logout, login computer before attempting to flash NuttX targets"
|
||||
fi
|
||||
|
||||
@@ -155,7 +155,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
|
||||
|
||||
if [ -n "$USER" ]; then
|
||||
# add user to dialout group (serial port access)
|
||||
sudo usermod -a -G dialout $USER
|
||||
sudo usermod -aG dialout $USER
|
||||
fi
|
||||
|
||||
# arm-none-eabi-gcc
|
||||
|
||||
@@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 NUM_ACTUATOR_OUTPUTS = 16
|
||||
uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking
|
||||
uint32 noutputs # valid outputs
|
||||
|
||||
uint8 NUM_ACTUATOR_OUTPUTS = 16
|
||||
uint8 noutputs # valid outputs
|
||||
float32[16] output # output data, in natural output units
|
||||
|
||||
# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
|
||||
|
||||
@@ -31,4 +31,3 @@ bool mode_req_manual_control
|
||||
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
||||
|
||||
|
||||
@@ -3,5 +3,3 @@ uint64 timestamp # time since system start (microseconds)
|
||||
# broadcast message to request all registered arming checks to be reported
|
||||
|
||||
uint8 request_id
|
||||
|
||||
|
||||
|
||||
@@ -16,4 +16,3 @@ uint8 source_id # ID depending on source_type
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
||||
|
||||
# TOPICS config_overrides config_overrides_request
|
||||
|
||||
|
||||
@@ -17,6 +17,6 @@ float32[2] test_ratio
|
||||
bool innovation_rejected # true if the observation has been rejected
|
||||
bool fused # true if the sample was successfully fused
|
||||
|
||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
|
||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
|
||||
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
|
||||
# TOPICS estimator_aid_src_drag
|
||||
|
||||
@@ -42,6 +42,7 @@ bool cs_gravity_vector # 34 - true when gravity vector measurements are
|
||||
bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
||||
bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used
|
||||
bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter
|
||||
bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended
|
||||
|
||||
# fault status
|
||||
uint32 fault_status_changes # number of filter fault status (fs) changes
|
||||
|
||||
@@ -22,4 +22,3 @@ float32 max_vertical_speed # (optional) [m/s] maximum speed (absolute) in the D-
|
||||
|
||||
bool flag_set_max_heading_rate # true if setting a non-default heading rate limit
|
||||
float32 max_heading_rate # (optional) [rad/s] maximum heading rate (absolute)
|
||||
|
||||
|
||||
@@ -7,4 +7,3 @@ uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase
|
||||
uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
|
||||
|
||||
char[50] topic_name # E.g. /fmu/in/vehicle_command
|
||||
|
||||
|
||||
@@ -14,3 +14,4 @@ float32 min_ground_speed_ref # minimum forward ground speed reference [m/s]
|
||||
float32 adapted_period # adapted period (if auto-tuning enabled) [s]
|
||||
float32 p_gain # controller proportional gain [rad/s]
|
||||
float32 time_const # controller time constant [s]
|
||||
float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]
|
||||
|
||||
@@ -11,4 +11,3 @@ int8 mode_id # assigned mode ID (-1 if invalid)
|
||||
int8 mode_executor_id # assigned mode executor ID (-1 if invalid)
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 2
|
||||
|
||||
|
||||
@@ -62,6 +62,4 @@ uint8 RTCM_MSG_USED_NOT_USED = 1
|
||||
uint8 RTCM_MSG_USED_USED = 2
|
||||
uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver
|
||||
|
||||
float32[3] position_offset # position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
|
||||
|
||||
# TOPICS sensor_gps vehicle_gps_position
|
||||
|
||||
@@ -5,6 +5,3 @@ char[25] name # either the mode name, or component name
|
||||
int8 arming_check_id # arming check registration ID (-1 if not registered)
|
||||
int8 mode_id # assigned mode ID (-1 if not registered)
|
||||
int8 mode_executor_id # assigned mode executor ID (-1 if not registered)
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -27,3 +27,4 @@ bool dead_reckoning # True if this position is estimated through dead-reckoning
|
||||
|
||||
# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
|
||||
# TOPICS estimator_global_position
|
||||
# TOPICS aux_global_position
|
||||
|
||||
@@ -71,7 +71,7 @@ const px4_spi_bus_t *px4_spi_buses{nullptr};
|
||||
|
||||
int px4_find_spi_bus(uint32_t devid)
|
||||
{
|
||||
for (int i = 0; (px4_spi_bus_t *) px4_spi_buses != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
|
||||
for (int i = 0; ((px4_spi_bus_t *) px4_spi_buses) != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
|
||||
const px4_spi_bus_t &bus_data = px4_spi_buses[i];
|
||||
|
||||
if (bus_data.bus == -1) {
|
||||
|
||||
@@ -53,7 +53,10 @@ public:
|
||||
|
||||
TEST_F(uORBMessageFieldsTest, decompressed_formats_match)
|
||||
{
|
||||
char buffer[1500];
|
||||
char buffer[1600];
|
||||
static_assert(uORB::orb_untokenized_fields_max_length < sizeof(buffer) - HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_),
|
||||
"msg definition too long / buffer too short");
|
||||
|
||||
uORB::MessageFormatReader format_reader(buffer, sizeof(buffer));
|
||||
|
||||
px4::Bitset<ORB_TOPICS_COUNT> formats_found;
|
||||
|
||||
@@ -877,7 +877,7 @@ void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control)
|
||||
}
|
||||
}
|
||||
|
||||
void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
|
||||
void ModalIo::mix_turtle_mode(float outputs[MAX_ACTUATORS])
|
||||
{
|
||||
bool use_pitch = true;
|
||||
bool use_roll = true;
|
||||
@@ -1052,8 +1052,7 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
|
||||
}
|
||||
|
||||
/* OutputModuleInterface */
|
||||
bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated)
|
||||
bool ModalIo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
{
|
||||
if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) {
|
||||
return false;
|
||||
|
||||
@@ -75,8 +75,7 @@ public:
|
||||
int print_status() override;
|
||||
|
||||
/** @see OutputModuleInterface */
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
|
||||
|
||||
virtual int init();
|
||||
|
||||
@@ -225,6 +224,6 @@ private:
|
||||
int parse_response(uint8_t *buf, uint8_t len, bool print_feedback);
|
||||
int flush_uart_rx();
|
||||
int check_for_esc_timeout();
|
||||
void mix_turtle_mode(uint16_t outputs[]);
|
||||
void mix_turtle_mode(float outputs[]);
|
||||
void handle_actuator_test();
|
||||
};
|
||||
|
||||
@@ -38,4 +38,4 @@ px4_add_module(
|
||||
ads1115_main.cpp
|
||||
ADS1115.cpp
|
||||
DEPENDS
|
||||
)
|
||||
)
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_ADC_BOARD_ADC
|
||||
bool "board_adc"
|
||||
default n
|
||||
---help---
|
||||
Enable support for board_adc
|
||||
Enable support for board_adc
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_BMP280
|
||||
bool "bmp280"
|
||||
default n
|
||||
---help---
|
||||
Enable support for bmp280
|
||||
Enable support for bmp280
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_BMP388
|
||||
bool "bmp388"
|
||||
default n
|
||||
---help---
|
||||
Enable support for bmp388
|
||||
Enable support for bmp388
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_DPS310
|
||||
bool "dps310"
|
||||
default n
|
||||
---help---
|
||||
Enable support for dps310
|
||||
Enable support for dps310
|
||||
|
||||
@@ -3,5 +3,3 @@ menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP10100
|
||||
default n
|
||||
---help---
|
||||
Enable support for icp10100
|
||||
|
||||
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_LPS22HB
|
||||
bool "lps22hb"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lps22hb
|
||||
Enable support for lps22hb
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_LPS25H
|
||||
bool "lps25h"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lps25h
|
||||
Enable support for lps25h
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_LPS33HW
|
||||
bool "lps33hw"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lps33hw
|
||||
Enable support for lps33hw
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_MPL3115A2
|
||||
bool "mpl3115a2"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mpl3115a2
|
||||
Enable support for mpl3115a2
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_MS5611
|
||||
bool "ms5611"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ms5611
|
||||
Enable support for ms5611
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BAROMETER_TCBP001TA
|
||||
bool "tcbp001ta"
|
||||
default n
|
||||
---help---
|
||||
Enable support for tcbp001ta
|
||||
Enable support for tcbp001ta
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BATT_SMBUS
|
||||
bool "batt_smbus"
|
||||
default n
|
||||
---help---
|
||||
Enable support for batt_smbus
|
||||
Enable support for batt_smbus
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_BOOTLOADERS
|
||||
bool "bootloaders"
|
||||
default n
|
||||
---help---
|
||||
Enable support for bootloaders
|
||||
Enable support for bootloaders
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_CAMERA_CAPTURE
|
||||
bool "camera_capture"
|
||||
default n
|
||||
---help---
|
||||
Enable support for camera_capture
|
||||
Enable support for camera_capture
|
||||
|
||||
@@ -84,4 +84,4 @@ PARAM_DEFINE_INT32(CAM_CAP_MODE, 0);
|
||||
* @group Camera Control
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);
|
||||
PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_CAMERA_TRIGGER
|
||||
bool "camera_trigger"
|
||||
default n
|
||||
---help---
|
||||
Enable support for camera_trigger
|
||||
Enable support for camera_trigger
|
||||
|
||||
@@ -97,7 +97,7 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
{
|
||||
if (_port_id > 0) {
|
||||
reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
|
||||
@@ -105,7 +105,7 @@ public:
|
||||
|
||||
for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
|
||||
if (i < num_outputs) {
|
||||
msg_sp.value[i] = static_cast<float>(outputs[i]);
|
||||
msg_sp.value[i] = outputs[i];
|
||||
|
||||
} else {
|
||||
// "unset" values published as NaN
|
||||
|
||||
@@ -392,8 +392,7 @@ void CyphalNode::sendHeartbeat()
|
||||
}
|
||||
}
|
||||
|
||||
bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
bool UavcanMixingInterface::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
{
|
||||
// Note: This gets called from MixingOutput from within its update() function
|
||||
// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect
|
||||
|
||||
@@ -87,8 +87,7 @@ public:
|
||||
_node_mutex(node_mutex),
|
||||
_pub_manager(pub_manager) {}
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
|
||||
|
||||
void printInfo() { _mixing_output.printStatus(); }
|
||||
|
||||
|
||||
@@ -39,8 +39,6 @@
|
||||
*@author Denislav Petrov <denislavamitoba@gmail.com>
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "ASP5033.hpp"
|
||||
|
||||
ASP5033::ASP5033(const I2CSPIDriverConfig &config) :
|
||||
@@ -80,9 +78,6 @@ int ASP5033::sensor_id_check()
|
||||
if ((transfer(&cmd_3, 1, &id[0], sizeof(id)) != PX4_OK) || (*id != REG_WHOAMI_RECHECK_ID_ASP5033)) { return 0; }
|
||||
|
||||
return 1;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
int ASP5033::init()
|
||||
@@ -123,13 +118,11 @@ bool ASP5033::get_differential_pressure()
|
||||
press_sum = 0.;
|
||||
press_count = 0;
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void ASP5033::print_status()
|
||||
{
|
||||
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
perf_print_counter(_sample_perf);
|
||||
@@ -209,7 +202,6 @@ int ASP5033::collect()
|
||||
perf_begin(_sample_perf);
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
|
||||
// Read pressure and temperature as one block
|
||||
uint8_t val[5] {0, 0, 0, 0, 0};
|
||||
uint8_t cmd = REG_PRESS_DATA_ASP5033;
|
||||
@@ -251,8 +243,3 @@ int ASP5033::collect()
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -31,7 +31,6 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* ASP5033 differential pressure sensor (external I2C)
|
||||
*
|
||||
@@ -40,7 +39,3 @@
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_ASP5033, 0);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_ETS
|
||||
bool "ets"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ets
|
||||
Enable support for ets
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X
|
||||
bool "sdp3x"
|
||||
default n
|
||||
---help---
|
||||
Enable support for sdp3x
|
||||
Enable support for sdp3x
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_CM8JL65
|
||||
bool "cm8jl65"
|
||||
default n
|
||||
---help---
|
||||
Enable support for cm8jl65
|
||||
Enable support for cm8jl65
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE
|
||||
bool "leddar_one"
|
||||
default n
|
||||
---help---
|
||||
Enable support for leddar_one
|
||||
Enable support for leddar_one
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C
|
||||
bool "lightware_laser_i2c"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lightware_laser_i2c
|
||||
Enable support for lightware_laser_i2c
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL
|
||||
bool "lightware_laser_serial"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lightware_laser_serial
|
||||
Enable support for lightware_laser_serial
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_LL40LS
|
||||
bool "ll40ls"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ll40ls
|
||||
Enable support for ll40ls
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_LL40LS_PWM
|
||||
bool "ll40ls_pwm"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ll40ls_pwm
|
||||
Enable support for ll40ls_pwm
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_MAPPYDOT
|
||||
bool "mappydot"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mappydot
|
||||
Enable support for mappydot
|
||||
|
||||
@@ -294,4 +294,4 @@ PARAM_DEFINE_INT32(SENS_MPDT10_ROT, 0);
|
||||
* @value 6 Yaw 270°
|
||||
* @value 7 Yaw 315°
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_MPDT11_ROT, 0);
|
||||
PARAM_DEFINE_INT32(SENS_MPDT11_ROT, 0);
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_MB12XX
|
||||
bool "mb12xx"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mb12xx
|
||||
Enable support for mb12xx
|
||||
|
||||
@@ -291,4 +291,4 @@ PARAM_DEFINE_INT32(SENS_MB12_10_ROT, 0);
|
||||
* @value 6 Yaw 270°
|
||||
* @value 7 Yaw 315°
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0);
|
||||
PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0);
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_PGA460
|
||||
bool "pga460"
|
||||
default n
|
||||
---help---
|
||||
Enable support for pga460
|
||||
Enable support for pga460
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_SRF02
|
||||
bool "srf02"
|
||||
default n
|
||||
---help---
|
||||
Enable support for srf02
|
||||
Enable support for srf02
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_SRF05
|
||||
bool "srf05"
|
||||
default n
|
||||
---help---
|
||||
Enable support for srf05
|
||||
Enable support for srf05
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_TERARANGER
|
||||
bool "teraranger"
|
||||
default n
|
||||
---help---
|
||||
Enable support for teraranger
|
||||
Enable support for teraranger
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_TFMINI
|
||||
bool "tfmini"
|
||||
default n
|
||||
---help---
|
||||
Enable support for tfmini
|
||||
Enable support for tfmini
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_ULANDING_RADAR
|
||||
bool "ulanding_radar"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ulanding_radar
|
||||
Enable support for ulanding_radar
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_VL53L0X
|
||||
bool "vl53l0x"
|
||||
default n
|
||||
---help---
|
||||
Enable support for vl53l0x
|
||||
Enable support for vl53l0x
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DISTANCE_SENSOR_VL53L1X
|
||||
bool "vl53l1x"
|
||||
default n
|
||||
---help---
|
||||
Enable support for vl53l1x
|
||||
Enable support for vl53l1x
|
||||
|
||||
@@ -336,8 +336,7 @@ void DShot::mixerChanged()
|
||||
update_telemetry_num_motors();
|
||||
}
|
||||
|
||||
bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated)
|
||||
bool DShot::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
{
|
||||
if (!_outputs_on) {
|
||||
return false;
|
||||
@@ -388,7 +387,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
|
||||
for (int i = 0; i < (int)num_outputs; i++) {
|
||||
|
||||
uint16_t output = outputs[i];
|
||||
uint16_t output = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
|
||||
|
||||
if (output == DSHOT_DISARM_VALUE) {
|
||||
up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
|
||||
|
||||
@@ -93,8 +93,7 @@ public:
|
||||
|
||||
bool telemetry_enabled() const { return _telemetry != nullptr; }
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
|
||||
|
||||
private:
|
||||
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_DSHOT
|
||||
bool "dshot"
|
||||
default n
|
||||
---help---
|
||||
Enable support for dshot
|
||||
Enable support for dshot
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_HEATER
|
||||
bool "heater"
|
||||
default n
|
||||
---help---
|
||||
Enable support for heater
|
||||
Enable support for heater
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM20602
|
||||
bool "icm20602"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm20602
|
||||
Enable support for icm20602
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM20649
|
||||
bool "icm20649"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm20649
|
||||
Enable support for icm20649
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM20689
|
||||
bool "icm20689"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm20689
|
||||
Enable support for icm20689
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM20948
|
||||
bool "icm20948"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm20948
|
||||
Enable support for icm20948
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM40609D
|
||||
bool "icm40609d"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm40609d
|
||||
Enable support for icm40609d
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM42605
|
||||
bool "icm42605"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm42605
|
||||
Enable support for icm42605
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM42688P
|
||||
bool "icm42688p"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm42688p
|
||||
Enable support for icm42688p
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_ICM45686
|
||||
bool "icm45686"
|
||||
default n
|
||||
---help---
|
||||
Enable support for icm45686
|
||||
Enable support for icm45686
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_MPU6000
|
||||
bool "mpu6000"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mpu6000
|
||||
Enable support for mpu6000
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_MPU6500
|
||||
bool "mpu6500"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mpu6500
|
||||
Enable support for mpu6500
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IMU_INVENSENSE_MPU9250
|
||||
bool "mpu9250"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mpu9250
|
||||
Enable support for mpu9250
|
||||
|
||||
@@ -667,4 +667,4 @@ typedef enum
|
||||
VNPROCESSOR_IMU /**< IMU Processor. */
|
||||
} VnProcessorType;
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_IRLOCK
|
||||
bool "irlock"
|
||||
default n
|
||||
---help---
|
||||
Enable support for irlock
|
||||
Enable support for irlock
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_LIGHTS_NEOPIXEL
|
||||
bool "neopixel"
|
||||
default n
|
||||
---help---
|
||||
Enable support for neopixel
|
||||
Enable support for neopixel
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_LIGHTS_RGBLED
|
||||
bool "rgbled"
|
||||
default n
|
||||
---help---
|
||||
Enable support for rgbled
|
||||
Enable support for rgbled
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_LIGHTS_RGBLED_IS31FL3195
|
||||
bool "rgbled is31fl3195"
|
||||
default n
|
||||
---help---
|
||||
Enable support for rgbled IS3FL3195 driver
|
||||
Enable support for rgbled IS3FL3195 driver
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_LIGHTS_RGBLED_NCP5623C
|
||||
bool "rgbled_ncp5623c"
|
||||
default n
|
||||
---help---
|
||||
Enable support for rgbled_ncp5623c
|
||||
Enable support for rgbled_ncp5623c
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_LIGHTS_RGBLED_PWM
|
||||
bool "rgbled_pwm"
|
||||
default n
|
||||
---help---
|
||||
Enable support for rgbled_pwm
|
||||
Enable support for rgbled_pwm
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_LINUX_PWM_OUT
|
||||
bool "linux_pwm_out"
|
||||
default n
|
||||
---help---
|
||||
Enable support for linux_pwm_out
|
||||
Enable support for linux_pwm_out
|
||||
|
||||
@@ -95,10 +95,16 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated)
|
||||
bool LinuxPWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
{
|
||||
_pwm_out->send_output_pwm(outputs, num_outputs);
|
||||
uint16_t out[MAX_ACTUATORS] {};
|
||||
|
||||
for (unsigned i = 0; i < num_outputs; ++i) {
|
||||
out[i] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
|
||||
}
|
||||
|
||||
_pwm_out->send_output_pwm(out, num_outputs);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@@ -71,8 +71,7 @@ public:
|
||||
|
||||
int init();
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs) override;
|
||||
|
||||
private:
|
||||
static constexpr int MAX_ACTUATORS = 8;
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user