dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of https://github.com/PX4/Firmware into bottledrop

This commit is contained in:
Juchli D 2013-11-15 10:34:12 +01:00
parent adee479782 2116966b1e
commit 4b8c3c38cd
39 changed files with 307 additions and 336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
View File

@ -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
#

5
ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
View File

@ -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
#

5
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
View File

@ -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
#

5
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
View File

@ -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
#

5
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
View File

@ -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
#

35
ROMFS/px4fmu_common/init.d/100_mpx_easystar
View File

@ -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

40
ROMFS/px4fmu_common/init.d/101_hk_bixler
View File

@ -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

5
ROMFS/px4fmu_common/init.d/10_dji_f330
View File

@ -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

2
ROMFS/px4fmu_common/init.d/11_dji_f450
View File

@ -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

15
ROMFS/px4fmu_common/init.d/12-13_hex
View File

@ -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

10
ROMFS/px4fmu_common/init.d/15_tbs_discovery
View File

@ -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

7
ROMFS/px4fmu_common/init.d/16_3dr_iris
View File

@ -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)
@ -69,8 +67,7 @@ pwm rate -c 1234 -r 400
# 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 1050
#
# Start common for all multirotors apps

42
ROMFS/px4fmu_common/init.d/30_io_camflyer
View File

@ -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

42
ROMFS/px4fmu_common/init.d/31_io_phantom
View File

@ -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

31
ROMFS/px4fmu_common/init.d/32_skywalker_x5
View File

@ -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

43
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
View File

@ -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

9
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
View File

@ -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
@ -102,13 +102,6 @@ else
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi
#
# 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
#
# Start common for all multirotors apps
#

4
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
View File

@ -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

34
ROMFS/px4fmu_common/init.d/rc.fixedwing Normal file
View File

@ -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

2
ROMFS/px4fmu_common/init.d/rc.multirotor
View File

@ -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
#
#

5
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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

2
src/drivers/airspeed/airspeed.h
View File

@ -119,7 +119,7 @@ protected:
virtual int collect() = 0;
work_s _work;
uint16_t _max_differential_pressure_pa;
float _max_differential_pressure_pa;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;

17
src/drivers/boards/px4fmu-v1/px4fmu_init.c
View File

@ -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 */

2
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -180,7 +180,7 @@ ETSAirspeed::collect()
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = diff_pres_pa;
report.differential_pressure_pa = (float)diff_pres_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;

10
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -36,6 +36,7 @@
* @author Lorenz Meier
* @author Sarthak Kaingade
* @author Simon Wilks
* @author Thomas Gubler
*
* Driver for the MEAS Spec series connected via I2C.
*
@ -76,6 +77,7 @@
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
@ -184,7 +186,7 @@ MEASAirspeed::collect()
//diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
dp_raw = 0x3FFF & dp_raw;
dp_raw = 0x3FFF & dp_raw; //mask the used bits
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
@ -193,7 +195,11 @@ MEASAirspeed::collect()
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value
uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset);
struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed).

5
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -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) {

7
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -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) {

9
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -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

44
src/modules/commander/airspeed_calibration.cpp
View File

@ -37,12 +37,15 @@
*/
#include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "commander_helper.h"
#include <stdio.h>
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
@ -55,10 +58,13 @@
#endif
static const int ERROR = -1;
static const char *sensor_name = "dpress";
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
const int calibration_count = 2500;
@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
0.0f,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, "airspeed calibration done");
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral();
close(diff_pres_sub);
return OK;
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}

17
src/modules/commander/commander.cpp
View File

@ -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);

67
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -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 {

1
src/modules/mavlink/missionlib.c
View File

@ -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"

28
src/modules/mavlink/orb_listener.c
View File

@ -75,6 +75,7 @@ struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct airspeed_s airspeed;
struct mavlink_subscriptions mavlink_subs;
@ -92,6 +93,8 @@ static unsigned int gps_counter;
*/
static uint64_t last_sensor_timestamp;
static hrt_abstime last_sent_vfr = 0;
static void *uorb_receive_thread(void *arg);
struct listener {
@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
if (gcs_link)
if (gcs_link) {
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
att.pitchspeed,
att.yawspeed);
/* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
}
}
attitude_counter++;
}
@ -681,17 +695,7 @@ l_home(const struct listener *l)
void
l_airspeed(const struct listener *l)
{
struct airspeed_s airspeed;
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.relative_alt;
float climb = -global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
void
@ -849,7 +853,7 @@ uorb_receive_start(void)
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
/* --- AIRSPEED / VFR / HUD --- */
/* --- AIRSPEED --- */
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */

52
src/modules/multirotor_att_control/multirotor_rate_control.c
View File

@ -59,31 +59,23 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
//float yawrate_awu;
//float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
//float attrate_awu;
//float attrate_lim;
float rate_lim;
};
@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
//param_t yawrate_awu;
//param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
//param_t attrate_awu;
//param_t attrate_lim;
};
/**
@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
return OK;
}
@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
//param_get(h->yawrate_awu, &(p->yawrate_awu));
//param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
//param_get(h->attrate_awu, &(p->attrate_awu));
//param_get(h->attrate_lim, &(p->attrate_lim));
return OK;
}
@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static PID_t yaw_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller);
pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
if (!isfinite(yaw_rate_control)) {
yaw_rate_control = 0.0f;
warnx("rej. NaN ctrl yaw");
}
/* run pitch, roll and yaw controllers */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;

5
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);

1
src/modules/sensors/sensors.cpp
View File

@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced)
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor");
close(fd);
}
#if 0

2
src/modules/systemlib/airspeed.c
View File

@ -59,7 +59,7 @@
float calc_indicated_airspeed(float differential_pressure)
{
if (differential_pressure > 0) {
if (differential_pressure > 0.0f) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);

4
src/modules/uORB/topics/differential_pressure.h
View File

@ -54,8 +54,8 @@
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint16_t differential_pressure_pa; /**< Differential pressure reading */
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
float differential_pressure_pa; /**< Differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor */

19
src/systemcmds/pwm/pwm.c
View File

@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[])
if (pwm_value == 0)
usage("no PWM value provided");
/* get current servo values */
struct pwm_output_values last_spos;
for (unsigned i = 0; i < servo_count; i++) {
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
if (ret != OK)
err(1, "PWM_SERVO_GET(%d)", i);
}
/* perform PWM output */
/* Open console directly to grab CTRL-C signal */
@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[])
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
/* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
}
warnx("User abort\n");
exit(0);
}