forked from Archive/PX4-Autopilot
Merge branch 'master' into yaw_pid_fix
This commit is contained in:
Anton Babushkin
commit
9f4dc0d154
|
|
@ -55,11 +55,6 @@ hil mode_pwm
|
|||
#
|
||||
param set MAV_TYPE 1
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# Check if we got an IO
|
||||
#
|
||||
|
|
|
|||
|
|
@ -60,11 +60,6 @@ hil mode_pwm
|
|||
#
|
||||
param set MAV_TYPE 2
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# Check if we got an IO
|
||||
#
|
||||
|
|
|
|||
|
|
@ -52,11 +52,6 @@ hil mode_pwm
|
|||
#
|
||||
param set MAV_TYPE 1
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# Check if we got an IO
|
||||
#
|
||||
|
|
|
|||
|
|
@ -60,11 +60,6 @@ hil mode_pwm
|
|||
#
|
||||
param set MAV_TYPE 2
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# Check if we got an IO
|
||||
#
|
||||
|
|
|
|||
|
|
@ -55,11 +55,6 @@ hil mode_pwm
|
|||
#
|
||||
param set MAV_TYPE 1
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# Check if we got an IO
|
||||
#
|
||||
|
|
|
|||
|
|
@ -52,8 +52,6 @@ then
|
|||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
|
|
@ -61,46 +59,27 @@ else
|
|||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
commander start
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensors and test them.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
#
|
||||
# Start logging (depends on sensors)
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Start GPS interface (depends on orb)
|
||||
#
|
||||
gps start
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
|
||||
then
|
||||
echo "Using FMU_RET mixer from sd card"
|
||||
echo "Using /fs/microsd/etc/mixers/FMU_RET.mix"
|
||||
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
|
||||
else
|
||||
echo "Using standard FMU_RET mixer"
|
||||
echo "Using /etc/mixers/FMU_RET.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
|
||||
fi
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
|
|
|
|||
|
|
@ -52,8 +52,6 @@ then
|
|||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
|
|
@ -61,39 +59,27 @@ else
|
|||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
commander start
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensors and test them.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
#
|
||||
# Start logging (depends on sensors)
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Start GPS interface (depends on orb)
|
||||
#
|
||||
gps start
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
|
||||
then
|
||||
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
|
||||
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
|
||||
else
|
||||
echo "Using /etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
|
||||
fi
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
|
|
|
|||
|
|
@ -59,10 +59,7 @@ then
|
|||
mavlink start
|
||||
usleep 5000
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Set PWM values for DJI ESCs
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
|
@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
|
|||
pwm rate -c 1234 -r 400
|
||||
|
||||
#
|
||||
# Set disarmed, min and max PWM signals
|
||||
# Set disarmed, min and max PWM signals (for DJI ESCs)
|
||||
#
|
||||
pwm disarmed -c 1234 -p 900
|
||||
pwm min -c 1234 -p 1200
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200
|
|||
pwm max -c 1234 -p 1800
|
||||
|
||||
#
|
||||
# Start common for all multirotors apps
|
||||
# Start common multirotor apps
|
||||
#
|
||||
sh /etc/init.d/rc.multirotor
|
||||
|
||||
|
|
|
|||
|
|
@ -61,10 +61,6 @@ then
|
|||
usleep 5000
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Set PWM values for DJI ESCs
|
||||
px4io idle 900 900 900 900 900 900
|
||||
px4io min 1200 1200 1200 1200 1200 1200
|
||||
px4io max 1900 1900 1900 1900 1900 1900
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
|
@ -77,12 +73,19 @@ fi
|
|||
#
|
||||
# Load mixer
|
||||
#
|
||||
mixer load /dev/pwm_output $MIXER
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
|
||||
|
||||
#
|
||||
# Set PWM output frequency to 400 Hz
|
||||
#
|
||||
pwm -u 400 -m 0xff
|
||||
pwm rate -c 123456 -r 400
|
||||
|
||||
#
|
||||
# Set disarmed, min and max PWM signals
|
||||
#
|
||||
pwm disarmed -c 123456 -p 900
|
||||
pwm min -c 123456 -p 1100
|
||||
pwm max -c 123456 -p 1900
|
||||
|
||||
#
|
||||
# Start common for all multirotors apps
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ fi
|
|||
# MAV_TYPE 2 = quadrotor
|
||||
#
|
||||
param set MAV_TYPE 2
|
||||
|
||||
|
||||
set EXIT_ON_END no
|
||||
|
||||
#
|
||||
|
|
@ -43,8 +43,6 @@ then
|
|||
mavlink start
|
||||
usleep 5000
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
|
|
@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
|
|||
pwm rate -c 1234 -r 400
|
||||
|
||||
#
|
||||
# Set disarmed, min and max PWM signals (for DJI ESCs)
|
||||
# Set disarmed, min and max PWM signals
|
||||
#
|
||||
pwm disarmed -c 1234 -p 900
|
||||
pwm min -c 1234 -p 1200
|
||||
pwm max -c 1234 -p 1800
|
||||
pwm min -c 1234 -p 1100
|
||||
pwm max -c 1234 -p 1900
|
||||
|
||||
#
|
||||
# Start common for all multirotors apps
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ fi
|
|||
# MAV_TYPE 2 = quadrotor
|
||||
#
|
||||
param set MAV_TYPE 2
|
||||
|
||||
|
||||
set EXIT_ON_END no
|
||||
|
||||
#
|
||||
|
|
@ -43,8 +43,6 @@ then
|
|||
mavlink start
|
||||
usleep 5000
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
|
|
|
|||
|
|
@ -30,8 +30,6 @@ then
|
|||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
|
|
@ -39,41 +37,29 @@ else
|
|||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
commander start
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensors and test them.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
#
|
||||
# Start logging (depends on sensors)
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Start GPS interface (depends on orb)
|
||||
#
|
||||
gps start
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
|
||||
then
|
||||
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
|
||||
else
|
||||
echo "Using /etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
|
||||
fi
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
||||
fi
|
||||
|
|
@ -52,8 +52,6 @@ then
|
|||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
|
|
@ -61,41 +59,29 @@ else
|
|||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
commander start
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensors and test them.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
#
|
||||
# Start logging (depends on sensors)
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Start GPS interface (depends on orb)
|
||||
#
|
||||
gps start
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
|
||||
then
|
||||
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
|
||||
else
|
||||
echo "Using /etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
|
||||
fi
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
||||
fi
|
||||
|
|
@ -30,8 +30,6 @@ then
|
|||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
commander start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
|
|
@ -39,32 +37,10 @@ else
|
|||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
commander start
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensors and test them.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
#
|
||||
# Start logging (depends on sensors)
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Start GPS interface (depends on orb)
|
||||
#
|
||||
gps start
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
|
|
@ -77,8 +53,11 @@ else
|
|||
echo "Using /etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
|
||||
fi
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!nsh
|
||||
|
||||
echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs"
|
||||
echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO"
|
||||
|
||||
#
|
||||
# Load default params for this platform
|
||||
|
|
@ -33,17 +33,27 @@ fi
|
|||
# MAV_TYPE 2 = quadrotor
|
||||
#
|
||||
param set MAV_TYPE 2
|
||||
|
||||
#
|
||||
# Start MAVLink
|
||||
#
|
||||
mavlink start -d /dev/ttyS0 -b 57600
|
||||
usleep 5000
|
||||
|
||||
set EXIT_ON_END no
|
||||
|
||||
#
|
||||
# Start PWM output
|
||||
# Start and configure PX4IO or FMU interface
|
||||
#
|
||||
fmu mode_pwm
|
||||
if px4io detect
|
||||
then
|
||||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
usleep 5000
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
usleep 5000
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Load mixer
|
||||
|
|
@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
|
|||
#
|
||||
pwm rate -c 1234 -r 400
|
||||
|
||||
#
|
||||
# Set disarmed, min and max PWM signals
|
||||
#
|
||||
pwm disarmed -c 1234 -p 900
|
||||
pwm min -c 1234 -p 1100
|
||||
pwm max -c 1234 -p 1900
|
||||
|
||||
#
|
||||
# Start common for all multirotors apps
|
||||
#
|
||||
sh /etc/init.d/rc.multirotor
|
||||
|
||||
# Exit, because /dev/ttyS0 is needed for MAVLink
|
||||
exit
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -84,10 +84,10 @@ then
|
|||
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
fmu mode_pwm
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
usleep 5000
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -60,9 +60,7 @@ then
|
|||
# Start MAVLink (depends on orb)
|
||||
mavlink start -d /dev/ttyS1 -b 57600
|
||||
usleep 5000
|
||||
|
||||
commander start
|
||||
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
else
|
||||
fmu mode_pwm
|
||||
|
|
|
|||
|
|
@ -0,0 +1,34 @@
|
|||
#!nsh
|
||||
#
|
||||
# Standard everything needed for fixedwing except mixer, actuator output and mavlink
|
||||
#
|
||||
|
||||
#
|
||||
# Start the sensors and test them.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
#
|
||||
# Start logging (depends on sensors)
|
||||
#
|
||||
sh /etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Start GPS interface (depends on orb)
|
||||
#
|
||||
gps start
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Start attitude controller
|
||||
#
|
||||
fw_att_control start
|
||||
|
||||
#
|
||||
# Start the position controller
|
||||
#
|
||||
fw_pos_control_l1 start
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
#!nsh
|
||||
#
|
||||
# Standard everything needed for multirotors except mixer, output and mavlink
|
||||
# Standard everything needed for multirotors except mixer, actuator output and mavlink
|
||||
#
|
||||
|
||||
#
|
||||
|
|
|
|||
|
|
@ -105,6 +105,11 @@ then
|
|||
blinkm systemstate
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the Commander (needs to be this early for in-air-restarts)
|
||||
#
|
||||
commander start
|
||||
|
||||
if param compare SYS_AUTOSTART 1000
|
||||
then
|
||||
|
|
|
|||
|
|
@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
|
|||
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
|
||||
* Keep the SPI2 init optional and conditionally initialize the ADC pins
|
||||
*/
|
||||
spi2 = up_spiinitialize(2);
|
||||
|
||||
if (!spi2) {
|
||||
message("[boot] Enabling IN12/13 instead of SPI2\n");
|
||||
/* no SPI2, use pins for ADC */
|
||||
stm32_configgpio(GPIO_ADC1_IN12);
|
||||
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
|
||||
} else {
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
spi2 = up_spiinitialize(2);
|
||||
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
|
||||
SPI_SETFREQUENCY(spi2, 10000000);
|
||||
SPI_SETBITS(spi2, 8);
|
||||
|
|
@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
|
|||
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
|
||||
|
||||
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
|
||||
}
|
||||
#else
|
||||
spi2 = NULL;
|
||||
message("[boot] Enabling IN12/13 instead of SPI2\n");
|
||||
/* no SPI2, use pins for ADC */
|
||||
stm32_configgpio(GPIO_ADC1_IN12);
|
||||
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
|
||||
#endif
|
||||
|
||||
/* Get the SPI port for the microSD slot */
|
||||
|
||||
|
|
|
|||
|
|
@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
|
|||
/* get the usual dt estimate */
|
||||
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
|
||||
_last_run = ecl_absolute_time();
|
||||
|
||||
float dt = dt_micros / 1000000;
|
||||
float dt = (float)dt_micros * 1e-6f;
|
||||
|
||||
/* lock integral for long intervals */
|
||||
if (dt_micros > 500000)
|
||||
|
|
@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
|
|||
|
||||
_rate_error = _rate_setpoint - pitch_rate;
|
||||
|
||||
float ilimit_scaled = 0.0f;
|
||||
float ilimit_scaled = _integrator_max * scaler;
|
||||
|
||||
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
|
||||
|
||||
|
|
|
|||
|
|
@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
|
|||
/* get the usual dt estimate */
|
||||
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
|
||||
_last_run = ecl_absolute_time();
|
||||
float dt = (float)dt_micros * 1e-6f;
|
||||
|
||||
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
|
||||
/* lock integral for long intervals */
|
||||
if (dt_micros > 500000)
|
||||
lock_integrator = true;
|
||||
|
||||
float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
|
||||
float k_i_rate = _k_i * _tc;
|
||||
|
|
@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
|
|||
_rate_error = _rate_setpoint - roll_rate;
|
||||
|
||||
|
||||
float ilimit_scaled = 0.0f;
|
||||
float ilimit_scaled = _integrator_max * scaler;
|
||||
|
||||
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
|
||||
|
||||
|
|
|
|||
|
|
@ -54,7 +54,9 @@ public:
|
|||
_SPE_est(0.0f),
|
||||
_SKE_est(0.0f),
|
||||
_SPEdot(0.0f),
|
||||
_SKEdot(0.0f) {
|
||||
_SKEdot(0.0f),
|
||||
_vel_dot(0.0f),
|
||||
_STEdotErrLast(0.0f) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -121,10 +121,10 @@ public:
|
|||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
float sig;
|
||||
int exp;
|
||||
int exponent;
|
||||
float num = (*this)(i, j);
|
||||
float2SigExp(num, sig, exp);
|
||||
printf("%6.3fe%03.3d,", (double)sig, exp);
|
||||
float2SigExp(num, sig, exponent);
|
||||
printf("%6.3fe%03d ", (double)sig, exponent);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
|
|
|||
|
|
@ -109,10 +109,10 @@ public:
|
|||
inline void print() const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
float sig;
|
||||
int exp;
|
||||
int exponent;
|
||||
float num = (*this)(i);
|
||||
float2SigExp(num, sig, exp);
|
||||
printf("%6.3fe%03.3d,", (double)sig, exp);
|
||||
float2SigExp(num, sig, exponent);
|
||||
printf("%6.3fe%03d ", (double)sig, exponent);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
/**
|
||||
* @file KalmanNav.cpp
|
||||
*
|
||||
* kalman filter navigation code
|
||||
* Kalman filter navigation code
|
||||
*/
|
||||
|
||||
#include <poll.h>
|
||||
|
|
@ -228,10 +228,7 @@ void KalmanNav::update()
|
|||
updateSubscriptions();
|
||||
|
||||
// initialize attitude when sensors online
|
||||
if (!_attitudeInitialized && sensorsUpdate &&
|
||||
_sensors.accelerometer_counter > 10 &&
|
||||
_sensors.gyro_counter > 10 &&
|
||||
_sensors.magnetometer_counter > 10) {
|
||||
if (!_attitudeInitialized && sensorsUpdate) {
|
||||
if (correctAtt() == ret_ok) _attitudeInitCounter++;
|
||||
|
||||
if (_attitudeInitCounter > 100) {
|
||||
|
|
@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
|
|||
|
||||
if (beta > _faultAtt.get()) {
|
||||
warnx("fault in attitude: beta = %8.4f", (double)beta);
|
||||
warnx("y:\n"); y.print();
|
||||
warnx("y:"); y.print();
|
||||
}
|
||||
|
||||
// update quaternions from euler
|
||||
|
|
|
|||
|
|
@ -116,6 +116,8 @@ extern struct system_load_s system_load;
|
|||
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
|
||||
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
|
||||
|
||||
#define MAVLINK_OPEN_INTERVAL 50000
|
||||
|
||||
#define STICK_ON_OFF_LIMIT 0.75f
|
||||
#define STICK_THRUST_RANGE 1.0f
|
||||
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
|
||||
|
|
@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
if (mavlink_fd < 0) {
|
||||
/* try again later */
|
||||
usleep(20000);
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
if (mavlink_fd < 0) {
|
||||
warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
|
||||
}
|
||||
}
|
||||
|
||||
/* Main state machine */
|
||||
/* make sure we are in preflight state */
|
||||
memset(&status, 0, sizeof(status));
|
||||
|
|
@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
|
||||
/* try to open the mavlink log device every once in a while */
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
|
||||
/* update parameters */
|
||||
orb_check(param_changed_sub, &updated);
|
||||
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd)
|
|||
|
||||
/* maximum 500 values */
|
||||
const unsigned int calibration_maxcount = 500;
|
||||
unsigned int calibration_counter = 0;
|
||||
unsigned int calibration_counter;
|
||||
|
||||
struct mag_scale mscale_null = {
|
||||
0.0f,
|
||||
|
|
@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd)
|
|||
res = ioctl(fd, MAGIOCCALIBRATE, fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
|
||||
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
|
||||
/* this is non-fatal - mark it accordingly */
|
||||
res = OK;
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
float *x;
|
||||
float *y;
|
||||
float *z;
|
||||
float *x = NULL;
|
||||
float *y = NULL;
|
||||
float *z = NULL;
|
||||
|
||||
if (res == OK) {
|
||||
/* allocate memory */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
|
||||
|
||||
x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
|
||||
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
|
||||
res = ERROR;
|
||||
return res;
|
||||
}
|
||||
} else {
|
||||
/* exit */
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
|
|
@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd)
|
|||
|
||||
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
|
||||
|
||||
calibration_counter = 0;
|
||||
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
|
||||
|
|
@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd)
|
|||
float sphere_radius;
|
||||
|
||||
if (res == OK) {
|
||||
|
||||
/* sphere fit */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
|
||||
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||
|
|
@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd)
|
|||
} else {
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||
_parameter_handles.p_i = param_find("FW_P_I");
|
||||
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
|
||||
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
|
||||
_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
|
||||
_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
|
||||
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
|
||||
|
||||
_parameter_handles.r_p = param_find("FW_R_P");
|
||||
_parameter_handles.r_d = param_find("FW_R_D");
|
||||
_parameter_handles.r_i = param_find("FW_R_I");
|
||||
_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
|
||||
_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
|
||||
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
|
||||
|
||||
_parameter_handles.y_p = param_find("FW_Y_P");
|
||||
_parameter_handles.y_i = param_find("FW_Y_I");
|
||||
_parameter_handles.y_d = param_find("FW_Y_D");
|
||||
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
|
||||
_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
|
||||
_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
|
||||
|
||||
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
|
||||
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
|
||||
|
|
@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
|
||||
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
|
||||
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
|
||||
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
|
||||
|
||||
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
|
||||
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
|
||||
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
|
||||
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
|
||||
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
|
||||
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
|
||||
|
||||
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
|
||||
_parameters.airspeed_min, _parameters.airspeed_max, airspeed);
|
||||
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
|
||||
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
|
||||
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
|
||||
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
|
||||
|
||||
/* throttle passed through */
|
||||
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
|
||||
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
|
||||
_parameters.airspeed_min, _parameters.airspeed_max, airspeed);
|
||||
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
|
||||
|
||||
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
|
||||
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
|
||||
// _actuators.control[2], _actuators.control[3]);
|
||||
/* throttle passed through */
|
||||
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
|
||||
|
||||
/*
|
||||
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
|
||||
* only once available
|
||||
*/
|
||||
vehicle_rates_setpoint_s rates_sp;
|
||||
rates_sp.roll = _roll_ctrl.get_desired_rate();
|
||||
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
|
||||
rates_sp.yaw = 0.0f; // XXX not yet implemented
|
||||
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
|
||||
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
|
||||
// _actuators.control[2], _actuators.control[3]);
|
||||
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
/*
|
||||
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
|
||||
* only once available
|
||||
*/
|
||||
vehicle_rates_setpoint_s rates_sp;
|
||||
rates_sp.roll = _roll_ctrl.get_desired_rate();
|
||||
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
|
||||
rates_sp.yaw = 0.0f; // XXX not yet implemented
|
||||
|
||||
if (_rate_sp_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
if (_rate_sp_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@
|
|||
#include <systemlib/systemlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "geo/geo.h"
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
|
|
|
|||
|
|
@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp)
|
|||
} else {
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
if (sd != nullptr)
|
||||
if (sd != nullptr) {
|
||||
hrt_cancel(&sd->update_call);
|
||||
delete sd;
|
||||
}
|
||||
}
|
||||
|
||||
return CDev::close(filp);
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[])
|
|||
/* ---- ACCEL ---- */
|
||||
|
||||
close(fd);
|
||||
fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||
|
||||
if (ret != OK) {
|
||||
|
|
@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[])
|
|||
goto system_eval;
|
||||
}
|
||||
|
||||
/* check measurement result range */
|
||||
struct accel_report acc;
|
||||
ret = read(fd, &acc, sizeof(acc));
|
||||
|
||||
if (ret == sizeof(acc)) {
|
||||
/* evaluate values */
|
||||
if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) {
|
||||
warnx("accel with spurious values");
|
||||
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
|
||||
/* this is frickin' fatal */
|
||||
fail_on_error = true;
|
||||
system_ok = false;
|
||||
goto system_eval;
|
||||
}
|
||||
} else {
|
||||
warnx("accel read failed");
|
||||
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
|
||||
/* this is frickin' fatal */
|
||||
fail_on_error = true;
|
||||
system_ok = false;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
/* ---- GYRO ---- */
|
||||
|
||||
close(fd);
|
||||
|
|
@ -193,9 +216,10 @@ system_eval:
|
|||
/* stop alarm */
|
||||
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
|
||||
|
||||
/* switch on leds */
|
||||
/* switch off leds */
|
||||
led_on(leds, LED_BLUE);
|
||||
led_on(leds, LED_AMBER);
|
||||
close(leds);
|
||||
|
||||
if (fail_on_error) {
|
||||
/* exit with error message */
|
||||
|
|
|
|||
Loading…
Reference in New Issue