forked from Archive/PX4-Autopilot
Merge branch 'master' into mpc_in_flight_lock
This commit is contained in:
Anton Babushkin
commit
8a25c48071
|
|
@ -14,19 +14,20 @@ sh /etc/init.d/rc.mc_defaults
|
|||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
param set MC_ROLL_P 5.0
|
||||
param set MC_ROLLRATE_P 0.13
|
||||
param set MC_ROLLRATE_I 0.0
|
||||
param set MC_ROLLRATE_D 0.0
|
||||
param set MC_PITCH_P 5.0
|
||||
param set MC_PITCHRATE_P 0.13
|
||||
param set MC_PITCHRATE_I 0.0
|
||||
param set MC_PITCHRATE_D 0.0
|
||||
param set MC_YAW_P 1.0
|
||||
param set MC_YAWRATE_P 0.15
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_ROLL_P 6.0
|
||||
param set MC_ROLLRATE_P 0.14
|
||||
param set MC_ROLLRATE_I 0.1
|
||||
param set MC_ROLLRATE_D 0.002
|
||||
param set MC_PITCH_P 6.0
|
||||
param set MC_PITCHRATE_P 0.14
|
||||
param set MC_PITCHRATE_I 0.1
|
||||
param set MC_PITCHRATE_D 0.002
|
||||
param set MC_YAW_P 2.0
|
||||
param set MC_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.2
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAW_FF 0.15
|
||||
param set MC_YAW_FF 0.8
|
||||
|
||||
param set BAT_V_SCALING 0.00838095238
|
||||
fi
|
||||
|
||||
|
|
|
|||
|
|
@ -37,10 +37,11 @@ then
|
|||
param set MPC_LAND_SPEED 1.0
|
||||
|
||||
param set PE_VELNE_NOISE 0.5
|
||||
param set PE_VELNE_NOISE 0.7
|
||||
param set PE_VELD_NOISE 0.7
|
||||
param set PE_POSNE_NOISE 0.5
|
||||
param set PE_POSD_NOISE 1.0
|
||||
|
||||
param set NAV_ACCEPT_RAD 2.0
|
||||
fi
|
||||
|
||||
set PWM_RATE 400
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
|
|||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
|
||||
usleep 100000
|
||||
mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
|
||||
usleep 100000
|
||||
|
||||
# Exit shell to make it available to MAVLink
|
||||
exit
|
||||
|
|
|
|||
|
|
@ -65,12 +65,12 @@ then
|
|||
# Start CDC/ACM serial driver
|
||||
#
|
||||
sercon
|
||||
|
||||
|
||||
#
|
||||
# Start the ORB (first app to start)
|
||||
#
|
||||
uorb start
|
||||
|
||||
|
||||
#
|
||||
# Load parameters
|
||||
#
|
||||
|
|
@ -79,7 +79,7 @@ then
|
|||
then
|
||||
set PARAM_FILE /fs/mtd_params
|
||||
fi
|
||||
|
||||
|
||||
param select $PARAM_FILE
|
||||
if param load
|
||||
then
|
||||
|
|
@ -87,21 +87,25 @@ then
|
|||
else
|
||||
echo "[init] ERROR: Params loading failed: $PARAM_FILE"
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# Start system state indicator
|
||||
#
|
||||
if rgbled start
|
||||
then
|
||||
echo "[init] Using external RGB Led"
|
||||
echo "[init] RGB Led"
|
||||
else
|
||||
if blinkm start
|
||||
then
|
||||
echo "[init] Using blinkm"
|
||||
echo "[init] BlinkM"
|
||||
blinkm systemstate
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
if pca8574 start
|
||||
then
|
||||
fi
|
||||
|
||||
#
|
||||
# Set default values
|
||||
#
|
||||
|
|
@ -122,7 +126,7 @@ then
|
|||
set LOAD_DEFAULT_APPS yes
|
||||
set GPS yes
|
||||
set GPS_FAKE no
|
||||
|
||||
|
||||
#
|
||||
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
|
||||
#
|
||||
|
|
@ -132,7 +136,7 @@ then
|
|||
else
|
||||
set DO_AUTOCONFIG no
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# Set USE_IO flag
|
||||
#
|
||||
|
|
@ -142,7 +146,7 @@ then
|
|||
else
|
||||
set USE_IO no
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# Set parameters and env variables for selected AUTOSTART
|
||||
#
|
||||
|
|
@ -172,9 +176,9 @@ then
|
|||
param set SYS_AUTOCONFIG 0
|
||||
param save
|
||||
fi
|
||||
|
||||
|
||||
set IO_PRESENT no
|
||||
|
||||
|
||||
if [ $USE_IO == yes ]
|
||||
then
|
||||
#
|
||||
|
|
@ -186,19 +190,19 @@ then
|
|||
else
|
||||
set IO_FILE /etc/extras/px4io-v1_default.bin
|
||||
fi
|
||||
|
||||
|
||||
if px4io checkcrc $IO_FILE
|
||||
then
|
||||
echo "[init] PX4IO CRC OK"
|
||||
echo "PX4IO CRC OK" >> $LOG_FILE
|
||||
|
||||
|
||||
set IO_PRESENT yes
|
||||
else
|
||||
echo "[init] Trying to update"
|
||||
echo "PX4IO Trying to update" >> $LOG_FILE
|
||||
|
||||
|
||||
tone_alarm MLL32CP8MB
|
||||
|
||||
|
||||
if px4io forceupdate 14662 $IO_FILE
|
||||
then
|
||||
usleep 500000
|
||||
|
|
@ -207,7 +211,7 @@ then
|
|||
echo "[init] PX4IO CRC OK, update successful"
|
||||
echo "PX4IO CRC OK after updating" >> $LOG_FILE
|
||||
tone_alarm MLL8CDE
|
||||
|
||||
|
||||
set IO_PRESENT yes
|
||||
else
|
||||
echo "[init] ERROR: PX4IO update failed"
|
||||
|
|
@ -220,14 +224,14 @@ then
|
|||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
if [ $IO_PRESENT == no ]
|
||||
then
|
||||
echo "[init] ERROR: PX4IO not found"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# Set default output if not set
|
||||
#
|
||||
|
|
@ -246,7 +250,7 @@ then
|
|||
# Need IO for output but it not present, disable output
|
||||
set OUTPUT_MODE none
|
||||
echo "[init] ERROR: PX4IO not found, disabling output"
|
||||
|
||||
|
||||
# Avoid using ttyS0 for MAVLink on FMUv1
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
|
|
@ -270,17 +274,17 @@ then
|
|||
|
||||
# Try to get an USB console
|
||||
nshterm /dev/ttyACM0 &
|
||||
|
||||
|
||||
#
|
||||
# Start the Commander (needs to be this early for in-air-restarts)
|
||||
#
|
||||
commander start
|
||||
|
||||
|
||||
#
|
||||
# Start primary output
|
||||
#
|
||||
set TTYS1_BUSY no
|
||||
|
||||
|
||||
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
|
||||
if [ $OUTPUT_MODE != none ]
|
||||
then
|
||||
|
|
@ -296,7 +300,7 @@ then
|
|||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
|
||||
then
|
||||
echo "[init] Use FMU as primary output"
|
||||
|
|
@ -307,7 +311,7 @@ then
|
|||
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
|
||||
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
|
||||
|
|
@ -320,7 +324,7 @@ then
|
|||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
if [ $OUTPUT_MODE == mkblctrl ]
|
||||
then
|
||||
echo "[init] Use MKBLCTRL as primary output"
|
||||
|
|
@ -333,7 +337,7 @@ then
|
|||
then
|
||||
set MKBLCTRL_ARG "-mkmode +"
|
||||
fi
|
||||
|
||||
|
||||
if mkblctrl $MKBLCTRL_ARG
|
||||
then
|
||||
echo "[init] MKBLCTRL started"
|
||||
|
|
@ -341,9 +345,9 @@ then
|
|||
echo "[init] ERROR: MKBLCTRL start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
|
||||
|
||||
fi
|
||||
|
||||
|
||||
if [ $OUTPUT_MODE == hil ]
|
||||
then
|
||||
echo "[init] Use HIL as primary output"
|
||||
|
|
@ -355,7 +359,7 @@ then
|
|||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# Start IO or FMU for RC PPM input if needed
|
||||
#
|
||||
|
|
@ -382,7 +386,7 @@ then
|
|||
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
|
||||
tone_alarm $TUNE_OUT_ERROR
|
||||
fi
|
||||
|
||||
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
|
||||
|
|
@ -397,7 +401,7 @@ then
|
|||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# MAVLink
|
||||
#
|
||||
|
|
@ -418,7 +422,7 @@ then
|
|||
fi
|
||||
|
||||
mavlink start $MAVLINK_FLAGS
|
||||
|
||||
|
||||
#
|
||||
# Sensors, Logging, GPS
|
||||
#
|
||||
|
|
@ -429,7 +433,7 @@ then
|
|||
echo "[init] Start logging"
|
||||
sh /etc/init.d/rc.logging
|
||||
fi
|
||||
|
||||
|
||||
if [ $GPS == yes ]
|
||||
then
|
||||
echo "[init] Start GPS"
|
||||
|
|
@ -439,7 +443,7 @@ then
|
|||
gps start -f
|
||||
else
|
||||
gps start
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
|
|
@ -456,24 +460,24 @@ then
|
|||
if [ $VEHICLE_TYPE == fw ]
|
||||
then
|
||||
echo "[init] Vehicle type: FIXED WING"
|
||||
|
||||
|
||||
if [ $MIXER == none ]
|
||||
then
|
||||
# Set default mixer for fixed wing if not defined
|
||||
set MIXER FMU_AERT
|
||||
fi
|
||||
|
||||
|
||||
if [ $MAV_TYPE == none ]
|
||||
then
|
||||
# Use MAV_TYPE = 1 (fixed wing) if not defined
|
||||
set MAV_TYPE 1
|
||||
fi
|
||||
|
||||
|
||||
param set MAV_TYPE $MAV_TYPE
|
||||
|
||||
|
||||
# Load mixer and configure outputs
|
||||
sh /etc/init.d/rc.interface
|
||||
|
||||
|
||||
# Start standard fixedwing apps
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
then
|
||||
|
|
@ -521,7 +525,7 @@ then
|
|||
set MAV_TYPE 14
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
# Still no MAV_TYPE found
|
||||
if [ $MAV_TYPE == none ]
|
||||
then
|
||||
|
|
@ -529,10 +533,10 @@ then
|
|||
else
|
||||
param set MAV_TYPE $MAV_TYPE
|
||||
fi
|
||||
|
||||
|
||||
# Load mixer and configure outputs
|
||||
sh /etc/init.d/rc.interface
|
||||
|
||||
|
||||
# Start standard multicopter apps
|
||||
if [ $LOAD_DEFAULT_APPS == yes ]
|
||||
then
|
||||
|
|
|
|||
|
|
@ -0,0 +1,154 @@
|
|||
PX4 mixer definitions
|
||||
=====================
|
||||
|
||||
Files in this directory implement example mixers that can be used as a basis
|
||||
for customisation, or for general testing purposes.
|
||||
|
||||
Mixer basics
|
||||
------------
|
||||
|
||||
Mixers combine control values from various sources (control tasks, user inputs,
|
||||
etc.) and produce output values suitable for controlling actuators; servos,
|
||||
motors, switches and so on.
|
||||
|
||||
An actuator derives its value from the combination of one or more control
|
||||
values. Each of the control values is scaled according to the actuator's
|
||||
configuration and then combined to produce the actuator value, which may then be
|
||||
further scaled to suit the specific output type.
|
||||
|
||||
Internally, all scaling is performed using floating point values. Inputs and
|
||||
outputs are clamped to the range -1.0 to 1.0.
|
||||
|
||||
control control control
|
||||
| | |
|
||||
v v v
|
||||
scale scale scale
|
||||
| | |
|
||||
| v |
|
||||
+-------> mix <------+
|
||||
|
|
||||
scale
|
||||
|
|
||||
v
|
||||
out
|
||||
|
||||
Scaling
|
||||
-------
|
||||
|
||||
Basic scalers provide linear scaling of the input to the output.
|
||||
|
||||
Each scaler allows the input value to be scaled independently for inputs
|
||||
greater/less than zero. An offset can be applied to the output, and lower and
|
||||
upper boundary constraints can be applied. Negative scaling factors cause the
|
||||
output to be inverted (negative input produces positive output).
|
||||
|
||||
Scaler pseudocode:
|
||||
|
||||
if (input < 0)
|
||||
output = (input * NEGATIVE_SCALE) + OFFSET
|
||||
else
|
||||
output = (input * POSITIVE_SCALE) + OFFSET
|
||||
|
||||
if (output < LOWER_LIMIT)
|
||||
output = LOWER_LIMIT
|
||||
if (output > UPPER_LIMIT)
|
||||
output = UPPER_LIMIT
|
||||
|
||||
Syntax
|
||||
------
|
||||
|
||||
Mixer definitions are text files; lines beginning with a single capital letter
|
||||
followed by a colon are significant. All other lines are ignored, meaning that
|
||||
explanatory text can be freely mixed with the definitions.
|
||||
|
||||
Each file may define more than one mixer; the allocation of mixers to actuators
|
||||
is specific to the device reading the mixer definition, and the number of
|
||||
actuator outputs generated by a mixer is specific to the mixer.
|
||||
|
||||
A mixer begins with a line of the form
|
||||
|
||||
<tag>: <mixer arguments>
|
||||
|
||||
The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a
|
||||
multirotor mixer, etc.
|
||||
|
||||
Null Mixer
|
||||
..........
|
||||
|
||||
A null mixer consumes no controls and generates a single actuator output whose
|
||||
value is always zero. Typically a null mixer is used as a placeholder in a
|
||||
collection of mixers in order to achieve a specific pattern of actuator outputs.
|
||||
|
||||
The null mixer definition has the form:
|
||||
|
||||
Z:
|
||||
|
||||
Simple Mixer
|
||||
............
|
||||
|
||||
A simple mixer combines zero or more control inputs into a single actuator
|
||||
output. Inputs are scaled, and the mixing function sums the result before
|
||||
applying an output scaler.
|
||||
|
||||
A simple mixer definition begins with:
|
||||
|
||||
M: <control count>
|
||||
O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
|
||||
|
||||
If <control count> is zero, the sum is effectively zero and the mixer will
|
||||
output a fixed value that is <offset> constrained by <lower limit> and <upper
|
||||
limit>.
|
||||
|
||||
The second line defines the output scaler with scaler parameters as discussed
|
||||
above. Whilst the calculations are performed as floating-point operations, the
|
||||
values stored in the definition file are scaled by a factor of 10000; i.e. an
|
||||
offset of -0.5 is encoded as -5000.
|
||||
|
||||
The definition continues with <control count> entries describing the control
|
||||
inputs and their scaling, in the form:
|
||||
|
||||
S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
|
||||
|
||||
The <group> value identifies the control group from which the scaler will read,
|
||||
and the <index> value an offset within that group. These values are specific to
|
||||
the device reading the mixer definition.
|
||||
|
||||
When used to mix vehicle controls, mixer group zero is the vehicle attitude
|
||||
control group, and index values zero through three are normally roll, pitch,
|
||||
yaw and thrust respectively.
|
||||
|
||||
The remaining fields on the line configure the control scaler with parameters as
|
||||
discussed above. Whilst the calculations are performed as floating-point
|
||||
operations, the values stored in the definition file are scaled by a factor of
|
||||
10000; i.e. an offset of -0.5 is encoded as -5000.
|
||||
|
||||
Multirotor Mixer
|
||||
................
|
||||
|
||||
The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
|
||||
into a set of actuator outputs intended to drive motor speed controllers.
|
||||
|
||||
The mixer definition is a single line of the form:
|
||||
|
||||
R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
|
||||
|
||||
The supported geometries include:
|
||||
|
||||
4x - quadrotor in X configuration
|
||||
4+ - quadrotor in + configuration
|
||||
6x - hexcopter in X configuration
|
||||
6+ - hexcopter in + configuration
|
||||
8x - octocopter in X configuration
|
||||
8+ - octocopter in + configuration
|
||||
|
||||
Each of the roll, pitch and yaw scale values determine scaling of the roll,
|
||||
pitch and yaw controls relative to the thrust control. Whilst the calculations
|
||||
are performed as floating-point operations, the values stored in the definition
|
||||
file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
|
||||
|
||||
Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
|
||||
thrust input ranges from 0.0 to 1.0. Output for each actuator is in the
|
||||
range -1.0 to 1.0.
|
||||
|
||||
In the case where an actuator saturates, all actuator values are rescaled so that
|
||||
the saturating actuator is limited to 1.0.
|
||||
|
|
@ -40,6 +40,7 @@ MODULES += drivers/meas_airspeed
|
|||
MODULES += drivers/frsky_telemetry
|
||||
MODULES += modules/sensors
|
||||
MODULES += drivers/mkblctrl
|
||||
MODULES += drivers/pca8574
|
||||
|
||||
|
||||
# Needs to be burned to the ground and re-written; for now,
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ MODULES += drivers/lsm303d
|
|||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/pca8574
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/tests
|
||||
|
|
|
|||
|
|
@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y
|
|||
#
|
||||
# USART1 Configuration
|
||||
#
|
||||
CONFIG_USART1_RXBUFSIZE=256
|
||||
CONFIG_USART1_TXBUFSIZE=128
|
||||
CONFIG_USART1_RXBUFSIZE=300
|
||||
CONFIG_USART1_TXBUFSIZE=300
|
||||
CONFIG_USART1_BAUD=57600
|
||||
CONFIG_USART1_BITS=8
|
||||
CONFIG_USART1_PARITY=0
|
||||
|
|
@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y
|
|||
#
|
||||
# UART5 Configuration
|
||||
#
|
||||
CONFIG_UART5_RXBUFSIZE=256
|
||||
CONFIG_UART5_TXBUFSIZE=128
|
||||
CONFIG_UART5_RXBUFSIZE=300
|
||||
CONFIG_UART5_TXBUFSIZE=300
|
||||
CONFIG_UART5_BAUD=57600
|
||||
CONFIG_UART5_BITS=8
|
||||
CONFIG_UART5_PARITY=0
|
||||
|
|
|
|||
|
|
@ -268,6 +268,10 @@
|
|||
#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
|
||||
|
||||
#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz)
|
||||
#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz)
|
||||
|
||||
/************************************************************************************
|
||||
* Public Data
|
||||
************************************************************************************/
|
||||
|
|
|
|||
|
|
@ -235,7 +235,7 @@ CONFIG_STM32_SDIO=y
|
|||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI2=y
|
||||
# CONFIG_STM32_SPI3 is not set
|
||||
# CONFIG_STM32_SPI4 is not set
|
||||
CONFIG_STM32_SPI4=y
|
||||
# CONFIG_STM32_SPI5 is not set
|
||||
# CONFIG_STM32_SPI6 is not set
|
||||
CONFIG_STM32_SYSCFG=y
|
||||
|
|
|
|||
|
|
@ -86,6 +86,7 @@ __BEGIN_DECLS
|
|||
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
|
||||
|
||||
#define PX4_SPI_BUS_SENSORS 1
|
||||
#define PX4_SPI_BUS_EXT 2
|
||||
|
||||
/*
|
||||
* Use these in place of the spi_dev_e enumeration to
|
||||
|
|
@ -98,7 +99,7 @@ __BEGIN_DECLS
|
|||
/*
|
||||
* Optional devices on IO's external port
|
||||
*/
|
||||
#define PX4_SPIDEV_ACCEL_MAG 2
|
||||
#define PX4_SPIDEV_ACCEL_MAG 2
|
||||
|
||||
/*
|
||||
* I2C busses
|
||||
|
|
|
|||
|
|
@ -106,8 +106,11 @@ __BEGIN_DECLS
|
|||
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
|
||||
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
|
||||
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
|
||||
#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
|
||||
#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
|
||||
|
||||
#define PX4_SPI_BUS_SENSORS 1
|
||||
#define PX4_SPI_BUS_EXT 4
|
||||
|
||||
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
|
||||
#define PX4_SPIDEV_GYRO 1
|
||||
|
|
@ -115,6 +118,10 @@ __BEGIN_DECLS
|
|||
#define PX4_SPIDEV_BARO 3
|
||||
#define PX4_SPIDEV_MPU 4
|
||||
|
||||
/* External bus */
|
||||
#define PX4_SPIDEV_EXT0 1
|
||||
#define PX4_SPIDEV_EXT1 2
|
||||
|
||||
/* I2C busses */
|
||||
#define PX4_I2C_BUS_EXPANSION 1
|
||||
#define PX4_I2C_BUS_LED 2
|
||||
|
|
|
|||
|
|
@ -192,6 +192,7 @@ stm32_boardinitialize(void)
|
|||
|
||||
static struct spi_dev_s *spi1;
|
||||
static struct spi_dev_s *spi2;
|
||||
static struct spi_dev_s *spi4;
|
||||
static struct sdio_dev_s *sdio;
|
||||
|
||||
#include <math.h>
|
||||
|
|
@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void)
|
|||
|
||||
message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
|
||||
|
||||
spi4 = up_spiinitialize(4);
|
||||
|
||||
/* Default SPI4 to 1MHz and de-assert the known chip selects. */
|
||||
SPI_SETFREQUENCY(spi4, 10000000);
|
||||
SPI_SETBITS(spi4, 8);
|
||||
SPI_SETMODE(spi4, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
|
||||
SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
|
||||
|
||||
message("[boot] Initialized SPI port 4\n");
|
||||
|
||||
#ifdef CONFIG_MMCSD
|
||||
/* First, get an instance of the SDIO interface */
|
||||
|
||||
|
|
|
|||
|
|
@ -94,6 +94,13 @@ __EXPORT void weak_function stm32_spiinitialize(void)
|
|||
stm32_configgpio(GPIO_SPI_CS_FRAM);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI4
|
||||
stm32_configgpio(GPIO_SPI_CS_EXT0);
|
||||
stm32_configgpio(GPIO_SPI_CS_EXT1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
|
||||
#endif
|
||||
}
|
||||
|
||||
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
|
|
@ -157,3 +164,31 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
|
|||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
|
||||
{
|
||||
/* SPI select is active low, so write !selected to select the device */
|
||||
|
||||
switch (devid) {
|
||||
case PX4_SPIDEV_EXT0:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
|
||||
break;
|
||||
|
||||
case PX4_SPIDEV_EXT1:
|
||||
/* Making sure the other peripherals are not selected */
|
||||
stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
|
||||
{
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,71 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file drv_io_expander.h
|
||||
*
|
||||
* IO expander device API
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*/
|
||||
|
||||
#define _IOXIOCBASE (0x2800)
|
||||
#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n))
|
||||
|
||||
/** set a bitmask (non-blocking) */
|
||||
#define IOX_SET_MASK _IOXIOC(1)
|
||||
|
||||
/** get a bitmask (blocking) */
|
||||
#define IOX_GET_MASK _IOXIOC(2)
|
||||
|
||||
/** set device mode (non-blocking) */
|
||||
#define IOX_SET_MODE _IOXIOC(3)
|
||||
|
||||
/** set constant values (non-blocking) */
|
||||
#define IOX_SET_VALUE _IOXIOC(4)
|
||||
|
||||
/* ... to IOX_SET_VALUE + 8 */
|
||||
|
||||
/* enum passed to RGBLED_SET_MODE ioctl()*/
|
||||
enum IOX_MODE {
|
||||
IOX_MODE_OFF,
|
||||
IOX_MODE_ON,
|
||||
IOX_MODE_TEST_OUT
|
||||
};
|
||||
|
|
@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
|||
_gps_position(gps_position),
|
||||
_configured(false),
|
||||
_waiting_for_ack(false),
|
||||
_got_posllh(false),
|
||||
_got_velned(false),
|
||||
_got_timeutc(false),
|
||||
_disable_cmd_last(0)
|
||||
{
|
||||
decode_init();
|
||||
|
|
@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
|
|||
bool handled = false;
|
||||
|
||||
while (true) {
|
||||
bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
|
||||
|
||||
/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
|
||||
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
|
||||
|
||||
if (ret < 0) {
|
||||
/* something went wrong when polling */
|
||||
|
|
@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
|
|||
|
||||
} else if (ret == 0) {
|
||||
/* return success after short delay after receiving a packet or timeout after long delay */
|
||||
if (handled) {
|
||||
if (ready_to_return) {
|
||||
_got_posllh = false;
|
||||
_got_velned = false;
|
||||
_got_timeutc = false;
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
|
|
@ -438,6 +445,7 @@ UBX::handle_message()
|
|||
|
||||
_rate_count_lat_lon++;
|
||||
|
||||
_got_posllh = true;
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
|
|
@ -447,8 +455,8 @@ UBX::handle_message()
|
|||
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
|
||||
|
||||
_gps_position->fix_type = packet->gpsFix;
|
||||
_gps_position->s_variance_m_s = packet->sAcc;
|
||||
_gps_position->p_variance_m = packet->pAcc;
|
||||
_gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s
|
||||
_gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m
|
||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||
|
||||
ret = 1;
|
||||
|
|
@ -482,6 +490,7 @@ UBX::handle_message()
|
|||
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
||||
_gps_position->timestamp_time = hrt_absolute_time();
|
||||
|
||||
_got_timeutc = true;
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
|
|
@ -557,6 +566,7 @@ UBX::handle_message()
|
|||
|
||||
_rate_count_vel++;
|
||||
|
||||
_got_velned = true;
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -397,6 +397,9 @@ private:
|
|||
struct vehicle_gps_position_s *_gps_position;
|
||||
bool _configured;
|
||||
bool _waiting_for_ack;
|
||||
bool _got_posllh;
|
||||
bool _got_velned;
|
||||
bool _got_timeutc;
|
||||
uint8_t _message_class_needed;
|
||||
uint8_t _message_id_needed;
|
||||
ubx_decode_state_t _decode_state;
|
||||
|
|
|
|||
|
|
@ -880,7 +880,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
|
|||
|
||||
/* manual measurement */
|
||||
_mag_reports->flush();
|
||||
measure();
|
||||
_mag->measure();
|
||||
|
||||
/* measurement will have generated a report, copy it out */
|
||||
if (_mag_reports->get(mrb))
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
#
|
||||
# PCA8574 driver for RGB LED
|
||||
#
|
||||
|
||||
MODULE_COMMAND = pca8574
|
||||
SRCS = pca8574.cpp
|
||||
|
|
@ -0,0 +1,554 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file pca8574.cpp
|
||||
*
|
||||
* Driver for an 8 I/O controller (PC8574) connected via I2C.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include <nuttx/wqueue.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include <drivers/drv_io_expander.h>
|
||||
|
||||
#define PCA8574_ONTIME 120
|
||||
#define PCA8574_OFFTIME 120
|
||||
#define PCA8574_DEVICE_PATH "/dev/pca8574"
|
||||
|
||||
#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND)
|
||||
|
||||
class PCA8574 : public device::I2C
|
||||
{
|
||||
public:
|
||||
PCA8574(int bus, int pca8574);
|
||||
virtual ~PCA8574();
|
||||
|
||||
|
||||
virtual int init();
|
||||
virtual int probe();
|
||||
virtual int info();
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
bool is_running() { return _running; }
|
||||
|
||||
private:
|
||||
work_s _work;
|
||||
|
||||
uint8_t _values_out;
|
||||
uint8_t _values_in;
|
||||
uint8_t _blinking;
|
||||
uint8_t _blink_phase;
|
||||
|
||||
enum IOX_MODE _mode;
|
||||
bool _running;
|
||||
int _led_interval;
|
||||
bool _should_run;
|
||||
bool _update_out;
|
||||
int _counter;
|
||||
|
||||
static void led_trampoline(void *arg);
|
||||
void led();
|
||||
|
||||
int send_led_enable(uint8_t arg);
|
||||
int send_led_values();
|
||||
|
||||
int get(uint8_t &vals);
|
||||
};
|
||||
|
||||
/* for now, we only support one PCA8574 */
|
||||
namespace
|
||||
{
|
||||
PCA8574 *g_pca8574;
|
||||
}
|
||||
|
||||
void pca8574_usage();
|
||||
|
||||
extern "C" __EXPORT int pca8574_main(int argc, char *argv[]);
|
||||
|
||||
PCA8574::PCA8574(int bus, int pca8574) :
|
||||
I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000),
|
||||
_values_out(0),
|
||||
_values_in(0),
|
||||
_blinking(0),
|
||||
_blink_phase(0),
|
||||
_mode(IOX_MODE_OFF),
|
||||
_running(false),
|
||||
_led_interval(80),
|
||||
_should_run(false),
|
||||
_update_out(false),
|
||||
_counter(0)
|
||||
{
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
PCA8574::~PCA8574()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::init()
|
||||
{
|
||||
int ret;
|
||||
ret = I2C::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::probe()
|
||||
{
|
||||
uint8_t val;
|
||||
return get(val);
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::info()
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = ENOTTY;
|
||||
|
||||
switch (cmd) {
|
||||
case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): {
|
||||
// set the specified on / off state
|
||||
uint8_t position = (1 << (cmd - IOX_SET_VALUE));
|
||||
uint8_t prev = _values_out;
|
||||
|
||||
if (arg) {
|
||||
_values_out |= position;
|
||||
|
||||
} else {
|
||||
_values_out &= ~(position);
|
||||
}
|
||||
|
||||
if (_values_out != prev) {
|
||||
if (_values_out) {
|
||||
_mode = IOX_MODE_ON;
|
||||
}
|
||||
send_led_values();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case IOX_SET_MASK:
|
||||
send_led_enable(arg);
|
||||
return OK;
|
||||
|
||||
case IOX_GET_MASK: {
|
||||
uint8_t val;
|
||||
ret = get(val);
|
||||
|
||||
if (ret == OK) {
|
||||
return val;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
case IOX_SET_MODE:
|
||||
|
||||
if (_mode != (IOX_MODE)arg) {
|
||||
|
||||
switch ((IOX_MODE)arg) {
|
||||
case IOX_MODE_OFF:
|
||||
_values_out = 0xFF;
|
||||
break;
|
||||
|
||||
case IOX_MODE_ON:
|
||||
_values_out = 0;
|
||||
break;
|
||||
|
||||
case IOX_MODE_TEST_OUT:
|
||||
break;
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
_mode = (IOX_MODE)arg;
|
||||
send_led_values();
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
||||
default:
|
||||
// see if the parent class can make any use of it
|
||||
ret = CDev::ioctl(filp, cmd, arg);
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
PCA8574::led_trampoline(void *arg)
|
||||
{
|
||||
PCA8574 *rgbl = reinterpret_cast<PCA8574 *>(arg);
|
||||
|
||||
rgbl->led();
|
||||
}
|
||||
|
||||
/**
|
||||
* Main loop function
|
||||
*/
|
||||
void
|
||||
PCA8574::led()
|
||||
{
|
||||
if (_mode == IOX_MODE_TEST_OUT) {
|
||||
|
||||
// we count only seven states
|
||||
_counter &= 0xF;
|
||||
_counter++;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (i < _counter) {
|
||||
_values_out |= (1 << i);
|
||||
|
||||
} else {
|
||||
_values_out &= ~(1 << i);
|
||||
}
|
||||
}
|
||||
|
||||
_update_out = true;
|
||||
_should_run = true;
|
||||
} else if (_mode == IOX_MODE_OFF) {
|
||||
_update_out = true;
|
||||
_should_run = false;
|
||||
} else {
|
||||
|
||||
// Any of the normal modes
|
||||
if (_blinking > 0) {
|
||||
/* we need to be running to blink */
|
||||
_should_run = true;
|
||||
} else {
|
||||
_should_run = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (_update_out) {
|
||||
uint8_t msg;
|
||||
|
||||
if (_blinking) {
|
||||
msg = (_values_out & _blinking & _blink_phase);
|
||||
|
||||
// wipe out all positions that are marked as blinking
|
||||
msg &= ~(_blinking);
|
||||
|
||||
// fill blink positions
|
||||
msg |= ((_blink_phase) ? _blinking : 0);
|
||||
|
||||
_blink_phase = !_blink_phase;
|
||||
} else {
|
||||
msg = _values_out;
|
||||
}
|
||||
|
||||
int ret = transfer(&msg, sizeof(msg), nullptr, 0);
|
||||
|
||||
if (!ret) {
|
||||
_update_out = false;
|
||||
}
|
||||
}
|
||||
|
||||
// check if any activity remains, else stp
|
||||
if (!_should_run) {
|
||||
_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// re-queue ourselves to run again later
|
||||
_running = true;
|
||||
work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sent ENABLE flag to LED driver
|
||||
*/
|
||||
int
|
||||
PCA8574::send_led_enable(uint8_t arg)
|
||||
{
|
||||
|
||||
int ret = transfer(&arg, sizeof(arg), nullptr, 0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Send 8 outputs
|
||||
*/
|
||||
int
|
||||
PCA8574::send_led_values()
|
||||
{
|
||||
_update_out = true;
|
||||
|
||||
// if not active, kick it
|
||||
if (!_running) {
|
||||
_running = true;
|
||||
work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::get(uint8_t &vals)
|
||||
{
|
||||
uint8_t result;
|
||||
int ret;
|
||||
|
||||
ret = transfer(nullptr, 0, &result, 1);
|
||||
|
||||
if (ret == OK) {
|
||||
_values_in = result;
|
||||
vals = result;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
pca8574_usage()
|
||||
{
|
||||
warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'");
|
||||
warnx("options:");
|
||||
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
|
||||
warnx(" -a addr (0x%x)", ADDR);
|
||||
}
|
||||
|
||||
int
|
||||
pca8574_main(int argc, char *argv[])
|
||||
{
|
||||
int i2cdevice = -1;
|
||||
int pca8574adr = ADDR; // 7bit
|
||||
|
||||
int ch;
|
||||
|
||||
// jump over start/off/etc and look at options first
|
||||
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'a':
|
||||
pca8574adr = strtol(optarg, NULL, 0);
|
||||
break;
|
||||
|
||||
case 'b':
|
||||
i2cdevice = strtol(optarg, NULL, 0);
|
||||
break;
|
||||
|
||||
default:
|
||||
pca8574_usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (optind >= argc) {
|
||||
pca8574_usage();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
if (g_pca8574 != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
if (i2cdevice == -1) {
|
||||
// try the external bus first
|
||||
i2cdevice = PX4_I2C_BUS_EXPANSION;
|
||||
g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr);
|
||||
|
||||
if (g_pca8574 != nullptr && OK != g_pca8574->init()) {
|
||||
delete g_pca8574;
|
||||
g_pca8574 = nullptr;
|
||||
}
|
||||
|
||||
if (g_pca8574 == nullptr) {
|
||||
// fall back to default bus
|
||||
if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
|
||||
errx(1, "init failed");
|
||||
}
|
||||
|
||||
i2cdevice = PX4_I2C_BUS_LED;
|
||||
}
|
||||
}
|
||||
|
||||
if (g_pca8574 == nullptr) {
|
||||
g_pca8574 = new PCA8574(i2cdevice, pca8574adr);
|
||||
|
||||
if (g_pca8574 == nullptr) {
|
||||
errx(1, "new failed");
|
||||
}
|
||||
|
||||
if (OK != g_pca8574->init()) {
|
||||
delete g_pca8574;
|
||||
g_pca8574 = nullptr;
|
||||
errx(1, "init failed");
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
// need the driver past this point
|
||||
if (g_pca8574 == nullptr) {
|
||||
warnx("not started, run pca8574 start");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "test")) {
|
||||
fd = open(PCA8574_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " PCA8574_DEVICE_PATH);
|
||||
}
|
||||
|
||||
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
|
||||
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "info")) {
|
||||
g_pca8574->info();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "off")) {
|
||||
fd = open(PCA8574_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
errx(1, "Unable to open " PCA8574_DEVICE_PATH);
|
||||
}
|
||||
|
||||
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
fd = open(PCA8574_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " PCA8574_DEVICE_PATH);
|
||||
}
|
||||
|
||||
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
|
||||
close(fd);
|
||||
|
||||
// wait until we're not running any more
|
||||
for (unsigned i = 0; i < 15; i++) {
|
||||
if (!g_pca8574->is_running()) {
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(50000);
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
}
|
||||
printf("\n");
|
||||
fflush(stdout);
|
||||
|
||||
if (!g_pca8574->is_running()) {
|
||||
delete g_pca8574;
|
||||
g_pca8574 = nullptr;
|
||||
exit(0);
|
||||
} else {
|
||||
warnx("stop failed.");
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "val")) {
|
||||
if (argc < 4) {
|
||||
errx(1, "Usage: pca8574 val <channel> <0 or 1>");
|
||||
}
|
||||
|
||||
fd = open(PCA8574_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
errx(1, "Unable to open " PCA8574_DEVICE_PATH);
|
||||
}
|
||||
|
||||
unsigned channel = strtol(argv[2], NULL, 0);
|
||||
unsigned val = strtol(argv[3], NULL, 0);
|
||||
|
||||
if (channel < 8) {
|
||||
ret = ioctl(fd, (IOX_SET_VALUE + channel), val);
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
pca8574_usage();
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() :
|
|||
_integrator(0.0f),
|
||||
_rate_error(0.0f),
|
||||
_rate_setpoint(0.0f),
|
||||
_bodyrate_setpoint(0.0f)
|
||||
_bodyrate_setpoint(0.0f),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
|
||||
{
|
||||
perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
|
||||
}
|
||||
|
||||
ECL_PitchController::~ECL_PitchController()
|
||||
|
|
|
|||
|
|
@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() :
|
|||
_integrator(0.0f),
|
||||
_rate_error(0.0f),
|
||||
_rate_setpoint(0.0f),
|
||||
_bodyrate_setpoint(0.0f)
|
||||
_bodyrate_setpoint(0.0f),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
|
||||
{
|
||||
perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
|
||||
}
|
||||
|
||||
ECL_RollController::~ECL_RollController()
|
||||
|
|
|
|||
|
|
@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() :
|
|||
_rate_error(0.0f),
|
||||
_rate_setpoint(0.0f),
|
||||
_bodyrate_setpoint(0.0f),
|
||||
_coordinated_min_speed(1.0f)
|
||||
_coordinated_min_speed(1.0f),
|
||||
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
|
||||
{
|
||||
perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
|
||||
}
|
||||
|
||||
ECL_YawController::~ECL_YawController()
|
||||
|
|
|
|||
|
|
@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
home.alt = global_position.alt;
|
||||
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
|
||||
mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
|
||||
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
|
||||
|
||||
/* announce new home position */
|
||||
if (home_pub > 0) {
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
// Define EKF_DEBUG here to enable the debug print calls
|
||||
// if the macro is not set, these will be completely
|
||||
// optimized out by the compiler.
|
||||
#define EKF_DEBUG
|
||||
//#define EKF_DEBUG
|
||||
|
||||
#ifdef EKF_DEBUG
|
||||
#include <stdio.h>
|
||||
|
|
|
|||
|
|
@ -50,149 +50,322 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
|||
* Controller parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
// @DisplayName Attitude Time Constant
|
||||
// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
|
||||
// @Range 0.4 to 1.0 seconds, in tens of seconds
|
||||
|
||||
/**
|
||||
* Attitude Time Constant
|
||||
*
|
||||
* This defines the latency between a step input and the achieved setpoint
|
||||
* (inverse to a P gain). Half a second is a good start value and fits for
|
||||
* most average systems. Smaller systems may require smaller values, but as
|
||||
* this will wear out servos faster, the value should only be decreased as
|
||||
* needed.
|
||||
*
|
||||
* @unit seconds
|
||||
* @min 0.4
|
||||
* @max 1.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
|
||||
|
||||
// @DisplayName Pitch rate proportional gain.
|
||||
// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error.
|
||||
// @Range 10 to 200, 1 increments
|
||||
/**
|
||||
* Pitch rate proportional gain.
|
||||
*
|
||||
* This defines how much the elevator input will be commanded depending on the
|
||||
* current body angular rate error.
|
||||
*
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
|
||||
|
||||
// @DisplayName Pitch rate integrator gain.
|
||||
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
|
||||
// @Range 0 to 50.0
|
||||
/**
|
||||
* Pitch rate integrator gain.
|
||||
*
|
||||
* This gain defines how much control response will result out of a steady
|
||||
* state error. It trims any constant error.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 50.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
|
||||
|
||||
// @DisplayName Maximum positive / up pitch rate.
|
||||
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
|
||||
// @Range 0 to 90.0 degrees per seconds, in 1 increments
|
||||
/**
|
||||
* Maximum positive / up pitch rate.
|
||||
*
|
||||
* This limits the maximum pitch up angular rate the controller will output (in
|
||||
* degrees per second). Setting a value of zero disables the limit.
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
|
||||
|
||||
// @DisplayName Maximum negative / down pitch rate.
|
||||
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
|
||||
// @Range 0 to 90.0 degrees per seconds, in 1 increments
|
||||
/**
|
||||
* Maximum negative / down pitch rate.
|
||||
*
|
||||
* This limits the maximum pitch down up angular rate the controller will
|
||||
* output (in degrees per second). Setting a value of zero disables the limit.
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
|
||||
|
||||
// @DisplayName Pitch rate integrator limit
|
||||
// @Description The portion of the integrator part in the control surface deflection is limited to this value
|
||||
// @Range 0.0 to 1
|
||||
// @Increment 0.1
|
||||
/**
|
||||
* Pitch rate integrator limit
|
||||
*
|
||||
* The portion of the integrator part in the control surface deflection is
|
||||
* limited to this value
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
|
||||
|
||||
// @DisplayName Roll to Pitch feedforward gain.
|
||||
// @Description This compensates during turns and ensures the nose stays level.
|
||||
// @Range 0.5 2.0
|
||||
// @Increment 0.05
|
||||
// @User User
|
||||
/**
|
||||
* Roll to Pitch feedforward gain.
|
||||
*
|
||||
* This compensates during turns and ensures the nose stays level.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 2.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
|
||||
|
||||
// @DisplayName Roll rate proportional Gain.
|
||||
// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error.
|
||||
// @Range 10.0 200.0
|
||||
// @Increment 10.0
|
||||
// @User User
|
||||
/**
|
||||
* Roll rate proportional Gain
|
||||
*
|
||||
* This defines how much the aileron input will be commanded depending on the
|
||||
* current body angular rate error.
|
||||
*
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
|
||||
|
||||
// @DisplayName Roll rate integrator Gain
|
||||
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
|
||||
// @Range 0.0 100.0
|
||||
// @Increment 5.0
|
||||
// @User User
|
||||
/**
|
||||
* Roll rate integrator Gain
|
||||
*
|
||||
* This gain defines how much control response will result out of a steady
|
||||
* state error. It trims any constant error.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 100.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
|
||||
|
||||
// @DisplayName Roll Integrator Anti-Windup
|
||||
// @Description The portion of the integrator part in the control surface deflection is limited to this value.
|
||||
// @Range 0.0 to 1.0
|
||||
// @Increment 0.1
|
||||
/**
|
||||
* Roll Integrator Anti-Windup
|
||||
*
|
||||
* The portion of the integrator part in the control surface deflection is limited to this value.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
|
||||
|
||||
// @DisplayName Maximum Roll Rate
|
||||
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
|
||||
// @Range 0 to 90.0 degrees per seconds
|
||||
// @Increment 1.0
|
||||
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
|
||||
/**
|
||||
* Maximum Roll Rate
|
||||
*
|
||||
* This limits the maximum roll rate the controller will output (in degrees per
|
||||
* second). Setting a value of zero disables the limit.
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
|
||||
|
||||
// @DisplayName Yaw rate proportional gain.
|
||||
// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error.
|
||||
// @Range 10 to 200, 1 increments
|
||||
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
|
||||
/**
|
||||
* Yaw rate proportional gain
|
||||
*
|
||||
* This defines how much the rudder input will be commanded depending on the
|
||||
* current body angular rate error.
|
||||
*
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
|
||||
|
||||
// @DisplayName Yaw rate integrator gain.
|
||||
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
|
||||
// @Range 0 to 50.0
|
||||
/**
|
||||
* Yaw rate integrator gain
|
||||
*
|
||||
* This gain defines how much control response will result out of a steady
|
||||
* state error. It trims any constant error.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 50.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
|
||||
|
||||
// @DisplayName Yaw rate integrator limit
|
||||
// @Description The portion of the integrator part in the control surface deflection is limited to this value
|
||||
// @Range 0.0 to 1
|
||||
// @Increment 0.1
|
||||
/**
|
||||
* Yaw rate integrator limit
|
||||
*
|
||||
* The portion of the integrator part in the control surface deflection is
|
||||
* limited to this value
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
|
||||
|
||||
// @DisplayName Maximum Yaw Rate
|
||||
// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
|
||||
// @Range 0 to 90.0 degrees per seconds
|
||||
// @Increment 1.0
|
||||
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
|
||||
/**
|
||||
* Maximum Yaw Rate
|
||||
*
|
||||
* This limits the maximum yaw rate the controller will output (in degrees per
|
||||
* second). Setting a value of zero disables the limit.
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
|
||||
|
||||
// @DisplayName Roll rate feed forward
|
||||
// @Description Direct feed forward from rate setpoint to control surface output
|
||||
// @Range 0 to 10
|
||||
// @Increment 0.1
|
||||
/**
|
||||
* Roll rate feed forward
|
||||
*
|
||||
* Direct feed forward from rate setpoint to control surface output
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
|
||||
|
||||
// @DisplayName Pitch rate feed forward
|
||||
// @Description Direct feed forward from rate setpoint to control surface output
|
||||
// @Range 0 to 10
|
||||
// @Increment 0.1
|
||||
/**
|
||||
* Pitch rate feed forward
|
||||
*
|
||||
* Direct feed forward from rate setpoint to control surface output
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
|
||||
|
||||
// @DisplayName Yaw rate feed forward
|
||||
// @Description Direct feed forward from rate setpoint to control surface output
|
||||
// @Range 0 to 10
|
||||
// @Increment 0.1
|
||||
/**
|
||||
* Yaw rate feed forward
|
||||
*
|
||||
* Direct feed forward from rate setpoint to control surface output
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
|
||||
|
||||
// @DisplayName Minimal speed for yaw coordination
|
||||
// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
|
||||
// @Range 0 to 90.0 degrees per seconds
|
||||
// @Increment 1.0
|
||||
/**
|
||||
* Minimal speed for yaw coordination
|
||||
*
|
||||
* For airspeeds above this value, the yaw rate is calculated for a coordinated
|
||||
* turn. Set to a very high value to disable.
|
||||
*
|
||||
* @unit m/s
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
|
||||
|
||||
/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
|
||||
/* Airspeed parameters:
|
||||
* The following parameters about airspeed are used by the attitude and the
|
||||
* position controller.
|
||||
* */
|
||||
|
||||
// @DisplayName Minimum Airspeed
|
||||
// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
|
||||
// @Range 0.0 to 30
|
||||
/**
|
||||
* Minimum Airspeed
|
||||
*
|
||||
* If the airspeed falls below this value, the TECS controller will try to
|
||||
* increase airspeed more aggressively.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 30.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
|
||||
|
||||
// @DisplayName Trim Airspeed
|
||||
// @Description The TECS controller tries to fly at this airspeed
|
||||
// @Range 0.0 to 30
|
||||
/**
|
||||
* Trim Airspeed
|
||||
*
|
||||
* The TECS controller tries to fly at this airspeed.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 30.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
|
||||
|
||||
// @DisplayName Maximum Airspeed
|
||||
// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
|
||||
// @Range 0.0 to 30
|
||||
/**
|
||||
* Maximum Airspeed
|
||||
*
|
||||
* If the airspeed is above this value, the TECS controller will try to decrease
|
||||
* airspeed more aggressively.
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 30.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
|
||||
|
||||
// @DisplayName Roll Setpoint Offset
|
||||
// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
|
||||
// @Range -90.0 to 90.0
|
||||
/**
|
||||
* Roll Setpoint Offset
|
||||
*
|
||||
* An airframe specific offset of the roll setpoint in degrees, the value is
|
||||
* added to the roll setpoint and should correspond to the typical cruise speed
|
||||
* of the airframe.
|
||||
*
|
||||
* @unit deg
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
|
||||
|
||||
// @DisplayName Pitch Setpoint Offset
|
||||
// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
|
||||
// @Range -90.0 to 90.0
|
||||
/**
|
||||
* Pitch Setpoint Offset
|
||||
*
|
||||
* An airframe specific offset of the pitch setpoint in degrees, the value is
|
||||
* added to the pitch setpoint and should correspond to the typical cruise
|
||||
* speed of the airframe.
|
||||
*
|
||||
* @unit deg
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
|
||||
|
||||
// @DisplayName Max Manual Roll
|
||||
// @Description Max roll for manual control in attitude stabilized mode
|
||||
// @Range 0.0 to 90.0
|
||||
/**
|
||||
* Max Manual Roll
|
||||
*
|
||||
* Max roll for manual control in attitude stabilized mode
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
|
||||
|
||||
// @DisplayName Max Manual Pitch
|
||||
// @Description Max pitch for manual control in attitude stabilized mode
|
||||
// @Range 0.0 to 90.0
|
||||
/**
|
||||
* Max Manual Pitch
|
||||
*
|
||||
* Max pitch for manual control in attitude stabilized mode
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0.0
|
||||
* @max 90.0
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
|
||||
|
|
|
|||
|
|
@ -232,8 +232,6 @@ private:
|
|||
|
||||
float throttle_land_max;
|
||||
|
||||
float loiter_hold_radius;
|
||||
|
||||
float heightrate_p;
|
||||
float speedrate_p;
|
||||
|
||||
|
|
@ -277,8 +275,6 @@ private:
|
|||
|
||||
param_t throttle_land_max;
|
||||
|
||||
param_t loiter_hold_radius;
|
||||
|
||||
param_t heightrate_p;
|
||||
param_t speedrate_p;
|
||||
|
||||
|
|
@ -441,7 +437,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
|
||||
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
|
||||
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
|
||||
_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
|
||||
|
||||
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
|
||||
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
|
||||
|
|
@ -513,7 +508,6 @@ FixedwingPositionControl::parameters_update()
|
|||
/* L1 control parameters */
|
||||
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
|
||||
param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
|
||||
param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
|
||||
|
||||
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
|
||||
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
|
||||
|
|
|
|||
|
|
@ -71,17 +71,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
|
||||
|
||||
/**
|
||||
* Default Loiter Radius
|
||||
*
|
||||
* This radius is used when no other loiter radius is set.
|
||||
*
|
||||
* @min 10.0
|
||||
* @max 100.0
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
|
||||
|
||||
/**
|
||||
* Cruise throttle
|
||||
*
|
||||
|
|
|
|||
|
|
@ -105,7 +105,8 @@ static struct file_operations fops;
|
|||
*/
|
||||
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
||||
static uint64_t last_write_times[6] = {0};
|
||||
static uint64_t last_write_success_times[6] = {0};
|
||||
static uint64_t last_write_try_times[6] = {0};
|
||||
|
||||
/*
|
||||
* Internal function to send the bytes through the right serial port
|
||||
|
|
@ -166,38 +167,40 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
if (instance->get_flow_control_enabled()
|
||||
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
||||
|
||||
if (buf_free == 0) {
|
||||
|
||||
if (last_write_times[(unsigned)channel] != 0 &&
|
||||
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
|
||||
|
||||
warnx("DISABLING HARDWARE FLOW CONTROL");
|
||||
instance->enable_flow_control(false);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* apparently there is space left, although we might be
|
||||
* partially overflooding the buffer already */
|
||||
last_write_times[(unsigned)channel] = hrt_absolute_time();
|
||||
/* Disable hardware flow control:
|
||||
* if no successful write since a defined time
|
||||
* and if the last try was not the last successful write
|
||||
*/
|
||||
if (last_write_try_times[(unsigned)channel] != 0 &&
|
||||
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
|
||||
last_write_success_times[(unsigned)channel] !=
|
||||
last_write_try_times[(unsigned)channel])
|
||||
{
|
||||
warnx("DISABLING HARDWARE FLOW CONTROL");
|
||||
instance->enable_flow_control(false);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* If the wait until transmit flag is on, only transmit after we've received messages.
|
||||
Otherwise, transmit all the time. */
|
||||
if (instance->should_transmit()) {
|
||||
last_write_try_times[(unsigned)channel] = hrt_absolute_time();
|
||||
|
||||
/* check if there is space in the buffer, let it overflow else */
|
||||
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
|
||||
|
||||
if (desired > buf_free) {
|
||||
desired = buf_free;
|
||||
if (buf_free < desired) {
|
||||
/* we don't want to send anything just in half, so return */
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t ret = write(uart, ch, desired);
|
||||
if (ret != desired) {
|
||||
warnx("TX FAIL");
|
||||
} else {
|
||||
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -222,6 +225,8 @@ Mavlink::Mavlink() :
|
|||
_subscriptions(nullptr),
|
||||
_streams(nullptr),
|
||||
_mission_pub(-1),
|
||||
_mode(MAVLINK_MODE_NORMAL),
|
||||
_total_counter(0),
|
||||
_verbose(false),
|
||||
_forwarding_on(false),
|
||||
_passing_on(false),
|
||||
|
|
@ -415,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
|
|||
void
|
||||
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
|
||||
{
|
||||
|
||||
|
||||
Mavlink *inst;
|
||||
LL_FOREACH(_mavlink_instances, inst) {
|
||||
if (inst != self) {
|
||||
|
|
@ -785,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
|
|||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
|
||||
/* Start sending parameters */
|
||||
mavlink_pm_start_queued_send();
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
|
||||
mavlink_param_request_list_t req;
|
||||
mavlink_msg_param_request_list_decode(msg, &req);
|
||||
if (req.target_system == mavlink_system.sysid &&
|
||||
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
|
||||
/* Start sending parameters */
|
||||
mavlink_pm_start_queued_send();
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
|
||||
}
|
||||
} break;
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET: {
|
||||
|
|
@ -886,11 +896,12 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
|
|||
|
||||
switch (mavlink_mission_item->command) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
mission_item->pitch_min = mavlink_mission_item->param2;
|
||||
mission_item->pitch_min = mavlink_mission_item->param1;
|
||||
break;
|
||||
|
||||
default:
|
||||
mission_item->acceptance_radius = mavlink_mission_item->param2;
|
||||
mission_item->time_inside = mavlink_mission_item->param1;
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -899,7 +910,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
|
|||
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
|
||||
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
|
||||
|
||||
mission_item->time_inside = mavlink_mission_item->param1;
|
||||
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
||||
// mission_item->index = mavlink_mission_item->seq;
|
||||
mission_item->origin = ORIGIN_MAVLINK;
|
||||
|
|
@ -918,11 +928,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
|
|||
|
||||
switch (mission_item->nav_cmd) {
|
||||
case NAV_CMD_TAKEOFF:
|
||||
mavlink_mission_item->param2 = mission_item->pitch_min;
|
||||
mavlink_mission_item->param1 = mission_item->pitch_min;
|
||||
break;
|
||||
|
||||
default:
|
||||
mavlink_mission_item->param2 = mission_item->acceptance_radius;
|
||||
mavlink_mission_item->param1 = mission_item->time_inside;
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -933,7 +944,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
|
|||
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
|
||||
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
|
||||
mavlink_mission_item->command = mission_item->nav_cmd;
|
||||
mavlink_mission_item->param1 = mission_item->time_inside;
|
||||
mavlink_mission_item->autocontinue = mission_item->autocontinue;
|
||||
// mavlink_mission_item->seq = mission_item->index;
|
||||
|
||||
|
|
@ -949,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
|
|||
state->current_partner_compid = 0;
|
||||
state->timestamp_lastaction = 0;
|
||||
state->timestamp_last_send_setpoint = 0;
|
||||
state->timestamp_last_send_request = 0;
|
||||
state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
|
||||
state->current_dataman_id = 0;
|
||||
}
|
||||
|
|
@ -1049,6 +1060,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
|
|||
|
||||
} else {
|
||||
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
|
||||
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
|
||||
|
||||
if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
|
||||
}
|
||||
|
|
@ -1064,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
|
|||
wpr.seq = seq;
|
||||
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
_wpm->timestamp_last_send_request = hrt_absolute_time();
|
||||
|
||||
if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
|
||||
|
||||
|
|
@ -1106,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
|
|||
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
_wpm->current_partner_sysid = 0;
|
||||
_wpm->current_partner_compid = 0;
|
||||
|
||||
} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
|
||||
/* try to get WP again after short timeout */
|
||||
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1168,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
|
||||
|
||||
if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -1205,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
|
||||
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
|
||||
}
|
||||
|
||||
} else {
|
||||
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
|
||||
|
||||
if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -1298,12 +1305,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
|
||||
|
||||
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
|
||||
|
||||
} else {
|
||||
|
||||
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
|
||||
|
||||
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1362,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
|
||||
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
|
||||
|
||||
if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
|
@ -1435,6 +1430,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
|
||||
if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
|
||||
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
|
||||
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
|
||||
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
|
|
@ -1466,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
} else {
|
||||
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
|
||||
}
|
||||
|
||||
} else {
|
||||
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
|
||||
|
||||
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -1506,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
|
||||
if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
|
||||
}
|
||||
|
||||
|
||||
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
|
||||
|
||||
mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
|
||||
|
||||
if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -93,6 +93,7 @@ struct mavlink_wpm_storage {
|
|||
uint8_t current_partner_compid;
|
||||
uint64_t timestamp_lastaction;
|
||||
uint64_t timestamp_last_send_setpoint;
|
||||
uint64_t timestamp_last_send_request;
|
||||
uint32_t timeout;
|
||||
int current_dataman_id;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -274,7 +274,7 @@ protected:
|
|||
status->onboard_control_sensors_health,
|
||||
status->load * 1000.0f,
|
||||
status->battery_voltage * 1000.0f,
|
||||
status->battery_current * 1000.0f,
|
||||
status->battery_current * 100.0f,
|
||||
status->battery_remaining * 100.0f,
|
||||
status->drop_rate_comm,
|
||||
status->errors_comm,
|
||||
|
|
@ -938,14 +938,14 @@ protected:
|
|||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
if (pos_sp_triplet_sub->update(t)) {
|
||||
mavlink_msg_global_position_setpoint_int_send(_channel,
|
||||
MAV_FRAME_GLOBAL,
|
||||
(int32_t)(pos_sp_triplet->current.lat * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.lon * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.alt * 1000),
|
||||
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
|
||||
}
|
||||
/* always send this message, even if it has not been updated */
|
||||
pos_sp_triplet_sub->update(t);
|
||||
mavlink_msg_global_position_setpoint_int_send(_channel,
|
||||
MAV_FRAME_GLOBAL,
|
||||
(int32_t)(pos_sp_triplet->current.lat * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.lon * 1e7),
|
||||
(int32_t)(pos_sp_triplet->current.alt * 1000),
|
||||
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
|
|||
}
|
||||
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
{
|
||||
/* Init if not done yet */
|
||||
init();
|
||||
|
|
@ -76,24 +76,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
|
|||
|
||||
|
||||
if (isRotarywing)
|
||||
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence);
|
||||
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
|
||||
else
|
||||
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence);
|
||||
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
{
|
||||
|
||||
return checkGeofence(dm_current, nMissionItems, geofence);
|
||||
return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
{
|
||||
/* Update fixed wing navigation capabilites */
|
||||
updateNavigationCapabilities();
|
||||
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
||||
|
||||
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence));
|
||||
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
|
|
@ -109,7 +109,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
|
|||
return false;
|
||||
}
|
||||
|
||||
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
|
||||
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
|
||||
return false;
|
||||
}
|
||||
|
|
@ -119,6 +119,36 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
|
|||
return true;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
|
||||
{
|
||||
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
|
||||
for (size_t i = 0; i < nMissionItems; i++) {
|
||||
static struct mission_item_s missionitem;
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(dm_current, i, &missionitem, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
if (throw_error) {
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
if (home_alt > missionitem.altitude) {
|
||||
if (throw_error) {
|
||||
mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
|
||||
return false;
|
||||
} else {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
|
||||
{
|
||||
/* Go through all mission items and search for a landing waypoint
|
||||
|
|
|
|||
|
|
@ -61,14 +61,15 @@ private:
|
|||
|
||||
/* Checks for all airframes */
|
||||
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
|
||||
|
||||
/* Checks specific to fixedwing airframes */
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
|
||||
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
|
||||
void updateNavigationCapabilities();
|
||||
|
||||
/* Checks specific to rotarywing airframes */
|
||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
|
||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
|
||||
public:
|
||||
|
||||
MissionFeasibilityChecker();
|
||||
|
|
@ -77,7 +78,7 @@ public:
|
|||
/*
|
||||
* Returns true if mission is feasible and false otherwise
|
||||
*/
|
||||
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
|
||||
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -519,7 +519,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
|
|||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
|
||||
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
|
||||
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt);
|
||||
|
||||
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
|
||||
|
||||
|
|
@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
|
|||
|
||||
/* XXX TODO count turns */
|
||||
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
|
||||
_mission_item.loiter_radius > 0.01f) {
|
||||
|
||||
|
|
@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
|
|||
acceptance_radius = _parameters.acceptance_radius;
|
||||
}
|
||||
|
||||
float dist = -1.0f;
|
||||
float dist_xy = -1.0f;
|
||||
float dist_z = -1.0f;
|
||||
|
||||
/* calculate AMSL altitude for this waypoint */
|
||||
float wp_alt_amsl = _mission_item.altitude;
|
||||
|
||||
if (_mission_item.altitude_is_relative)
|
||||
wp_alt_amsl += _home_pos.alt;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
|
||||
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
if (_do_takeoff) {
|
||||
if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
|
||||
/* require only altitude for takeoff */
|
||||
/* require only altitude for takeoff */
|
||||
if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
float dist = -1.0f;
|
||||
float dist_xy = -1.0f;
|
||||
float dist_z = -1.0f;
|
||||
|
||||
/* calculate AMSL altitude for this waypoint */
|
||||
float wp_alt_amsl = _mission_item.altitude;
|
||||
|
||||
if (_mission_item.altitude_is_relative)
|
||||
wp_alt_amsl += _home_pos.alt;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
|
||||
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
if (dist >= 0.0f && dist <= acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
|
|
@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
|
|||
}
|
||||
|
||||
if (_mission.current_mission_available()) {
|
||||
set_mission_item();
|
||||
if (_mission_item.autocontinue) {
|
||||
/* continue mission */
|
||||
set_mission_item();
|
||||
|
||||
} else {
|
||||
/* autocontinue disabled for this item */
|
||||
request_loiter_or_ready();
|
||||
}
|
||||
|
||||
} else {
|
||||
/* if no more mission items available then finish mission */
|
||||
|
|
|
|||
|
|
@ -9,15 +9,18 @@
|
|||
|
||||
#include "inertial_filter.h"
|
||||
|
||||
void inertial_filter_predict(float dt, float x[3])
|
||||
void inertial_filter_predict(float dt, float x[2], float acc)
|
||||
{
|
||||
if (isfinite(dt)) {
|
||||
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
|
||||
x[1] += x[2] * dt;
|
||||
if (!isfinite(acc)) {
|
||||
acc = 0.0f;
|
||||
}
|
||||
x[0] += x[1] * dt + acc * dt * dt / 2.0f;
|
||||
x[1] += acc * dt;
|
||||
}
|
||||
}
|
||||
|
||||
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
|
||||
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
|
||||
{
|
||||
if (isfinite(e) && isfinite(w) && isfinite(dt)) {
|
||||
float ewdt = e * w * dt;
|
||||
|
|
@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
|
|||
|
||||
if (i == 0) {
|
||||
x[1] += w * ewdt;
|
||||
x[2] += w * w * ewdt / 3.0;
|
||||
|
||||
} else if (i == 1) {
|
||||
x[2] += w * ewdt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,6 +8,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
void inertial_filter_predict(float dt, float x[3]);
|
||||
void inertial_filter_predict(float dt, float x[3], float acc);
|
||||
|
||||
void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
|
||||
|
|
|
|||
|
|
@ -71,19 +71,20 @@
|
|||
#include "inertial_filter.h"
|
||||
|
||||
#define MIN_VALID_W 0.00001f
|
||||
#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz
|
||||
#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
|
||||
static bool verbose_mode = false;
|
||||
|
||||
static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s
|
||||
static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s
|
||||
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
|
||||
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
|
||||
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
|
||||
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
|
||||
static const uint32_t updates_counter_len = 1000000;
|
||||
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
|
||||
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
|
||||
|
||||
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
|
||||
|
|
@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]);
|
|||
|
||||
static void usage(const char *reason);
|
||||
|
||||
static inline int min(int val1, int val2)
|
||||
{
|
||||
return (val1 < val2) ? val1 : val2;
|
||||
}
|
||||
|
||||
static inline int max(int val1, int val2)
|
||||
{
|
||||
return (val1 > val2) ? val1 : val2;
|
||||
}
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
|
|
@ -135,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[])
|
|||
|
||||
thread_should_exit = false;
|
||||
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
|
||||
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
|
||||
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
|
||||
position_estimator_inav_thread_main,
|
||||
(argv) ? (const char **) &argv[2] : (const char **) NULL);
|
||||
exit(0);
|
||||
|
|
@ -168,15 +179,15 @@ int position_estimator_inav_main(int argc, char *argv[])
|
|||
exit(1);
|
||||
}
|
||||
|
||||
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
|
||||
void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
|
||||
{
|
||||
FILE *f = fopen("/fs/microsd/inav.log", "a");
|
||||
|
||||
if (f) {
|
||||
char *s = malloc(256);
|
||||
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]);
|
||||
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
|
||||
fwrite(s, 1, n, f);
|
||||
n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
|
||||
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
|
||||
fwrite(s, 1, n, f);
|
||||
free(s);
|
||||
}
|
||||
|
|
@ -195,14 +206,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
mavlink_log_info(mavlink_fd, "[inav] started");
|
||||
|
||||
float x_est[3] = { 0.0f, 0.0f, 0.0f };
|
||||
float y_est[3] = { 0.0f, 0.0f, 0.0f };
|
||||
float z_est[3] = { 0.0f, 0.0f, 0.0f };
|
||||
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
|
||||
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
|
||||
float z_est[2] = { 0.0f, 0.0f }; // pos, vel
|
||||
|
||||
float eph = 1.0;
|
||||
float epv = 1.0;
|
||||
float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer
|
||||
float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer
|
||||
float R_gps[3][3]; // rotation matrix for GPS correction moment
|
||||
memset(est_buf, 0, sizeof(est_buf));
|
||||
memset(R_buf, 0, sizeof(R_buf));
|
||||
memset(R_gps, 0, sizeof(R_gps));
|
||||
int buf_ptr = 0;
|
||||
|
||||
float x_est_prev[3], y_est_prev[3], z_est_prev[3];
|
||||
static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
|
||||
static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation
|
||||
|
||||
float eph = max_eph_epv;
|
||||
float epv = 1.0f;
|
||||
|
||||
float eph_flow = 1.0f;
|
||||
|
||||
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
|
||||
memset(x_est_prev, 0, sizeof(x_est_prev));
|
||||
memset(y_est_prev, 0, sizeof(y_est_prev));
|
||||
memset(z_est_prev, 0, sizeof(z_est_prev));
|
||||
|
|
@ -241,7 +265,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
|
||||
|
||||
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
|
||||
float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
|
||||
float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
|
||||
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
|
||||
float corr_baro = 0.0f; // D
|
||||
float corr_gps[3][2] = {
|
||||
|
|
@ -257,9 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
float corr_flow[] = { 0.0f, 0.0f }; // N E
|
||||
float w_flow = 0.0f;
|
||||
|
||||
static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
|
||||
static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation
|
||||
|
||||
float sonar_prev = 0.0f;
|
||||
hrt_abstime flow_prev = 0; // time of last flow measurement
|
||||
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
|
||||
|
|
@ -341,8 +362,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
|
||||
/* mean calculation over several measurements */
|
||||
if (baro_init_cnt < baro_init_num) {
|
||||
baro_offset += sensor.baro_alt_meter;
|
||||
baro_init_cnt++;
|
||||
if (isfinite(sensor.baro_alt_meter)) {
|
||||
baro_offset += sensor.baro_alt_meter;
|
||||
baro_init_cnt++;
|
||||
}
|
||||
|
||||
} else {
|
||||
wait_baro = false;
|
||||
|
|
@ -418,19 +441,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
|
||||
/* transform acceleration vector from body frame to NED frame */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_NED[i] = 0.0f;
|
||||
acc[i] = 0.0f;
|
||||
|
||||
for (int j = 0; j < 3; j++) {
|
||||
accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
|
||||
acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
|
||||
}
|
||||
}
|
||||
|
||||
corr_acc[0] = accel_NED[0] - x_est[2];
|
||||
corr_acc[1] = accel_NED[1] - y_est[2];
|
||||
corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
|
||||
acc[2] += CONSTANTS_ONE_G;
|
||||
|
||||
} else {
|
||||
memset(corr_acc, 0, sizeof(corr_acc));
|
||||
memset(acc, 0, sizeof(acc));
|
||||
}
|
||||
|
||||
accel_timestamp = sensor.accelerometer_timestamp;
|
||||
|
|
@ -536,9 +557,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
w_flow *= 0.05f;
|
||||
}
|
||||
|
||||
flow_valid = true;
|
||||
/* under ideal conditions, on 1m distance assume EPH = 10cm */
|
||||
eph_flow = 0.1 / w_flow;
|
||||
|
||||
eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm
|
||||
flow_valid = true;
|
||||
|
||||
} else {
|
||||
w_flow = 0.0f;
|
||||
|
|
@ -597,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
|
||||
/* hysteresis for GPS quality */
|
||||
if (gps_valid) {
|
||||
if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
|
||||
if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
|
||||
gps_valid = false;
|
||||
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
|
||||
}
|
||||
|
||||
} else {
|
||||
if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
|
||||
if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
|
||||
gps_valid = true;
|
||||
reset_est = true;
|
||||
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
|
||||
|
|
@ -626,10 +648,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
|
||||
x_est[0] = 0.0f;
|
||||
x_est[1] = gps.vel_n_m_s;
|
||||
x_est[2] = accel_NED[0];
|
||||
y_est[0] = 0.0f;
|
||||
y_est[1] = gps.vel_e_m_s;
|
||||
y_est[2] = accel_NED[1];
|
||||
|
||||
local_pos.ref_lat = lat;
|
||||
local_pos.ref_lon = lon;
|
||||
|
|
@ -652,22 +672,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
if (reset_est) {
|
||||
x_est[0] = gps_proj[0];
|
||||
x_est[1] = gps.vel_n_m_s;
|
||||
x_est[2] = accel_NED[0];
|
||||
y_est[0] = gps_proj[1];
|
||||
y_est[1] = gps.vel_e_m_s;
|
||||
y_est[2] = accel_NED[1];
|
||||
}
|
||||
|
||||
/* calculate index of estimated values in buffer */
|
||||
int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
|
||||
if (est_i < 0) {
|
||||
est_i += EST_BUF_SIZE;
|
||||
}
|
||||
|
||||
/* calculate correction for position */
|
||||
corr_gps[0][0] = gps_proj[0] - x_est[0];
|
||||
corr_gps[1][0] = gps_proj[1] - y_est[0];
|
||||
corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
|
||||
corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
|
||||
corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
|
||||
corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
|
||||
|
||||
/* calculate correction for velocity */
|
||||
if (gps.vel_ned_valid) {
|
||||
corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
|
||||
corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
|
||||
corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
|
||||
corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
|
||||
corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
|
||||
corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
|
||||
|
||||
} else {
|
||||
corr_gps[0][1] = 0.0f;
|
||||
|
|
@ -675,8 +699,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
corr_gps[2][1] = 0.0f;
|
||||
}
|
||||
|
||||
eph = fminf(eph, gps.eph_m);
|
||||
epv = fminf(epv, gps.epv_m);
|
||||
/* save rotation matrix at this moment */
|
||||
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
|
||||
|
||||
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
|
||||
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
|
||||
|
|
@ -718,8 +742,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
t_prev = t;
|
||||
|
||||
/* increase EPH/EPV on each step */
|
||||
eph *= 1.0 + dt;
|
||||
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
|
||||
if (eph < max_eph_epv) {
|
||||
eph *= 1.0 + dt;
|
||||
}
|
||||
if (epv < max_eph_epv) {
|
||||
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
|
||||
}
|
||||
|
||||
/* use GPS if it's valid and reference position initialized */
|
||||
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
|
||||
|
|
@ -732,7 +760,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
xy_src_time = t;
|
||||
}
|
||||
|
||||
bool can_estimate_xy = eph < max_eph_epv * 1.5;
|
||||
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
|
||||
|
||||
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
|
||||
|
||||
|
|
@ -764,7 +792,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
corr_baro += offs_corr;
|
||||
}
|
||||
|
||||
/* accelerometer bias correction */
|
||||
/* accelerometer bias correction for GPS (use buffered rotation matrix) */
|
||||
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
if (use_gps_xy) {
|
||||
|
|
@ -778,6 +806,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
|
||||
}
|
||||
|
||||
/* transform error vector from NED frame to body frame */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
float c = 0.0f;
|
||||
|
||||
for (int j = 0; j < 3; j++) {
|
||||
c += R_gps[j][i] * accel_bias_corr[j];
|
||||
}
|
||||
|
||||
if (isfinite(c)) {
|
||||
acc_bias[i] += c * params.w_acc_bias * dt;
|
||||
}
|
||||
}
|
||||
|
||||
/* accelerometer bias correction for flow and baro (assume that there is no delay) */
|
||||
accel_bias_corr[0] = 0.0f;
|
||||
accel_bias_corr[1] = 0.0f;
|
||||
accel_bias_corr[2] = 0.0f;
|
||||
|
||||
if (use_flow) {
|
||||
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
|
||||
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
|
||||
|
|
@ -793,26 +839,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
c += att.R[j][i] * accel_bias_corr[j];
|
||||
}
|
||||
|
||||
acc_bias[i] += c * params.w_acc_bias * dt;
|
||||
if (isfinite(c)) {
|
||||
acc_bias[i] += c * params.w_acc_bias * dt;
|
||||
}
|
||||
}
|
||||
|
||||
/* inertial filter prediction for altitude */
|
||||
inertial_filter_predict(dt, z_est);
|
||||
inertial_filter_predict(dt, z_est, acc[2]);
|
||||
|
||||
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
memcpy(z_est, z_est_prev, sizeof(z_est));
|
||||
}
|
||||
|
||||
/* inertial filter correction for altitude */
|
||||
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
|
||||
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
|
||||
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
|
||||
|
||||
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
if (use_gps_z) {
|
||||
epv = fminf(epv, gps.epv_m);
|
||||
|
||||
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
|
||||
}
|
||||
|
||||
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
memcpy(z_est, z_est_prev, sizeof(z_est));
|
||||
memset(corr_acc, 0, sizeof(corr_acc));
|
||||
memset(corr_gps, 0, sizeof(corr_gps));
|
||||
corr_baro = 0;
|
||||
|
||||
|
|
@ -822,25 +873,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
|
||||
if (can_estimate_xy) {
|
||||
/* inertial filter prediction for position */
|
||||
inertial_filter_predict(dt, x_est);
|
||||
inertial_filter_predict(dt, y_est);
|
||||
inertial_filter_predict(dt, x_est, acc[0]);
|
||||
inertial_filter_predict(dt, y_est, acc[1]);
|
||||
|
||||
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
memcpy(x_est, x_est_prev, sizeof(x_est));
|
||||
memcpy(y_est, y_est_prev, sizeof(y_est));
|
||||
}
|
||||
|
||||
/* inertial filter correction for position */
|
||||
inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
|
||||
inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
|
||||
|
||||
if (use_flow) {
|
||||
eph = fminf(eph, eph_flow);
|
||||
|
||||
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
|
||||
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
|
||||
}
|
||||
|
||||
if (use_gps_xy) {
|
||||
eph = fminf(eph, gps.eph_m);
|
||||
|
||||
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
|
||||
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
|
||||
|
||||
|
|
@ -850,11 +902,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
memcpy(x_est, x_est_prev, sizeof(x_est));
|
||||
memcpy(y_est, y_est_prev, sizeof(y_est));
|
||||
memset(corr_acc, 0, sizeof(corr_acc));
|
||||
memset(corr_gps, 0, sizeof(corr_gps));
|
||||
memset(corr_flow, 0, sizeof(corr_flow));
|
||||
|
||||
|
|
@ -915,8 +966,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
if (t > pub_last + pub_interval) {
|
||||
if (t > pub_last + PUB_INTERVAL) {
|
||||
pub_last = t;
|
||||
|
||||
/* push current estimate to buffer */
|
||||
est_buf[buf_ptr][0][0] = x_est[0];
|
||||
est_buf[buf_ptr][0][1] = x_est[1];
|
||||
est_buf[buf_ptr][1][0] = y_est[0];
|
||||
est_buf[buf_ptr][1][1] = y_est[1];
|
||||
est_buf[buf_ptr][2][0] = z_est[0];
|
||||
est_buf[buf_ptr][2][1] = z_est[1];
|
||||
|
||||
/* push current rotation matrix to buffer */
|
||||
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
|
||||
|
||||
buf_ptr++;
|
||||
if (buf_ptr >= EST_BUF_SIZE) {
|
||||
buf_ptr = 0;
|
||||
}
|
||||
|
||||
/* publish local position */
|
||||
local_pos.xy_valid = can_estimate_xy;
|
||||
local_pos.v_xy_valid = can_estimate_xy;
|
||||
|
|
|
|||
|
|
@ -42,11 +42,9 @@
|
|||
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
|
||||
|
|
@ -57,16 +55,15 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
|
|||
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
|
||||
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
|
||||
|
||||
int parameters_init(struct position_estimator_inav_param_handles *h)
|
||||
{
|
||||
h->w_z_baro = param_find("INAV_W_Z_BARO");
|
||||
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
|
||||
h->w_z_acc = param_find("INAV_W_Z_ACC");
|
||||
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
|
||||
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
|
||||
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
|
||||
h->w_xy_acc = param_find("INAV_W_XY_ACC");
|
||||
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
|
||||
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
|
||||
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
|
||||
|
|
@ -77,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
|
|||
h->land_t = param_find("INAV_LAND_T");
|
||||
h->land_disp = param_find("INAV_LAND_DISP");
|
||||
h->land_thr = param_find("INAV_LAND_THR");
|
||||
h->delay_gps = param_find("INAV_DELAY_GPS");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -85,11 +83,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
|
|||
{
|
||||
param_get(h->w_z_baro, &(p->w_z_baro));
|
||||
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
|
||||
param_get(h->w_z_acc, &(p->w_z_acc));
|
||||
param_get(h->w_z_sonar, &(p->w_z_sonar));
|
||||
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
|
||||
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
|
||||
param_get(h->w_xy_acc, &(p->w_xy_acc));
|
||||
param_get(h->w_xy_flow, &(p->w_xy_flow));
|
||||
param_get(h->w_gps_flow, &(p->w_gps_flow));
|
||||
param_get(h->w_acc_bias, &(p->w_acc_bias));
|
||||
|
|
@ -100,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
|
|||
param_get(h->land_t, &(p->land_t));
|
||||
param_get(h->land_disp, &(p->land_disp));
|
||||
param_get(h->land_thr, &(p->land_thr));
|
||||
param_get(h->delay_gps, &(p->delay_gps));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,11 +43,9 @@
|
|||
struct position_estimator_inav_params {
|
||||
float w_z_baro;
|
||||
float w_z_gps_p;
|
||||
float w_z_acc;
|
||||
float w_z_sonar;
|
||||
float w_xy_gps_p;
|
||||
float w_xy_gps_v;
|
||||
float w_xy_acc;
|
||||
float w_xy_flow;
|
||||
float w_gps_flow;
|
||||
float w_acc_bias;
|
||||
|
|
@ -58,16 +56,15 @@ struct position_estimator_inav_params {
|
|||
float land_t;
|
||||
float land_disp;
|
||||
float land_thr;
|
||||
float delay_gps;
|
||||
};
|
||||
|
||||
struct position_estimator_inav_param_handles {
|
||||
param_t w_z_baro;
|
||||
param_t w_z_gps_p;
|
||||
param_t w_z_acc;
|
||||
param_t w_z_sonar;
|
||||
param_t w_xy_gps_p;
|
||||
param_t w_xy_gps_v;
|
||||
param_t w_xy_acc;
|
||||
param_t w_xy_flow;
|
||||
param_t w_gps_flow;
|
||||
param_t w_acc_bias;
|
||||
|
|
@ -78,6 +75,7 @@ struct position_estimator_inav_param_handles {
|
|||
param_t land_t;
|
||||
param_t land_disp;
|
||||
param_t land_thr;
|
||||
param_t delay_gps;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -44,15 +44,25 @@
|
|||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** global 'actuator output is live' control. */
|
||||
struct actuator_armed_s {
|
||||
|
||||
uint64_t timestamp;
|
||||
bool armed; /**< Set to true if system is armed */
|
||||
bool ready_to_arm; /**< Set to true if system is ready to be armed */
|
||||
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
|
||||
uint64_t timestamp; /**< Microseconds since system boot */
|
||||
bool armed; /**< Set to true if system is armed */
|
||||
bool ready_to_arm; /**< Set to true if system is ready to be armed */
|
||||
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(actuator_armed);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue