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

Merged master

This commit is contained in:
Lorenz Meier 2014-07-31 11:37:32 +02:00
parent 1b3a775e1b 75dfb0d84d
commit 93a822fee4
158 changed files with 5350 additions and 1904 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -4,3 +4,6 @@
[submodule "NuttX"]
path = NuttX
url = git://github.com/PX4/NuttX.git
[submodule "uavcan"]
path = uavcan
url = git://github.com/pavel-kirienko/uavcan.git

3
Makefile
View File

@ -212,6 +212,9 @@ endif
$(NUTTX_SRC):
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
$(UAVCAN_DIR):
$(Q) (./Tools/check_submodules.sh)
.PHONY: checksubmodules
checksubmodules:
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)

2
NuttX

@ -1 +1 @@
Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df
Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172

3
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -30,3 +30,6 @@ fi
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_MIN 1200
set PWM_MAX 1950

9
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
View File

@ -26,15 +26,6 @@ then
param set FW_RR_P 0.1
param set FW_R_LIM 45
param set FW_R_RMAX 0
param set FW_T_CLMB_MAX 5
param set FW_T_HRATE_P 0.02
param set FW_T_PTCH_DAMP 0
param set FW_T_RLL2THR 15
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 3
param set FW_T_VERT_ACC 7
param set FW_YR_FF 0.0
param set FW_YR_I 0
param set FW_YR_IMAX 0.2

6
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -30,13 +30,9 @@ then
param set FW_RR_P 0.08
param set FW_R_LIM 50
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_Q
set MIXER phantom
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4

4
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

@ -30,10 +30,6 @@ then
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_X5

4
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
View File

@ -33,10 +33,6 @@ then
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_Q

4
ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

@ -23,5 +23,5 @@ then
param set MC_YAWRATE_D 0.0
fi
set PWM_MIN 1175
set PWM_MAX 1900
set PWM_MIN 1200
set PWM_MAX 1950

4
ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

@ -24,5 +24,5 @@ then
param set MC_YAWRATE_D 0.0
fi
set PWM_MIN 1175
set PWM_MAX 1900
set PWM_MIN 1230
set PWM_MAX 1950

27
ROMFS/px4fmu_common/init.d/4012_quad_x_can Normal file
View File

@ -0,0 +1,27 @@
#!nsh
#
# F450-sized quadrotor with CAN
#
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
fi
set OUTPUT_MODE uavcan_esc

5
ROMFS/px4fmu_common/init.d/rc.autostart
View File

@ -136,6 +136,11 @@ then
sh /etc/init.d/4011_dji_f450
fi
if param compare SYS_AUTOSTART 4012
then
sh /etc/init.d/4012_quad_x_can
fi
if param compare SYS_AUTOSTART 4020
then
sh /etc/init.d/4020_hk_micro_pcb

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

@ -11,4 +11,8 @@ then
param set NAV_RTL_ALT 100
param set NAV_RTL_LAND_T -1
param set NAV_ACCEPT_RAD 50
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi

5
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -24,6 +24,11 @@ then
else
set OUTPUT_DEV /dev/pwm_output
fi
if [ $OUTPUT_MODE == uavcan_esc ]
then
set OUTPUT_DEV /dev/uavcan/esc
fi
if mixer load $OUTPUT_DEV $MIXER_FILE
then

39
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -6,28 +6,51 @@
ms5611 start
adc start
# Mag might be external
if hmc5883 start
if mpu6000 -X start
then
echo "[init] Using HMC5883"
fi
if mpu6000 start
then
echo "[init] Using MPU6000"
fi
if l3gd20 -X start
then
fi
if l3gd20 start
then
echo "[init] Using L3GD20(H)"
fi
# MAG selection
if param compare SENS_EXT_MAG 2
then
if hmc5883 -I start
then
fi
else
# Use only external as primary
if param compare SENS_EXT_MAG 1
then
if hmc5883 -X start
then
fi
else
# auto-detect the primary, prefer external
if hmc5883 start
then
fi
fi
fi
if ver hwcmp PX4FMU_V2
then
# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
if lsm303d -X start
then
fi
if lsm303d start
then
echo "[init] Using LSM303D"
fi
fi
@ -38,11 +61,9 @@ then
else
if ets_airspeed start
then
echo "[init] Using ETS airspeed sensor (bus 3)"
else
if ets_airspeed start -b 1
then
echo "[init] Using ETS airspeed sensor (bus 1)"
fi
fi
fi

6
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -5,6 +5,10 @@
mavlink start -r 10000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
usleep 100000
mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
usleep 100000
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
usleep 100000
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
@ -15,6 +19,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
usleep 100000
mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
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

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

@ -297,7 +297,17 @@ then
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ]
then
if [ $OUTPUT_MODE == io ]
if [ $OUTPUT_MODE == uavcan_esc ]
then
if uavcan start 1
then
echo "CAN UP"
else
echo "CAN ERR"
fi
fi
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
then
echo "[init] Use PX4IO PWM as primary output"
if px4io start

67
ROMFS/px4fmu_common/mixers/phantom.mix Normal file
View File

@ -0,0 +1,67 @@
Phantom FX-61 mixer
===================
This file defines mixers suitable for controlling a delta wing aircraft using
PX4/Pixhawk. The configuration assumes the elevon servos are connected to
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor are set so that pitch will have more travel than roll.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 6500 6500 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 -6500 -6500 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000

24
Tools/check_submodules.sh
View File

@ -53,4 +53,28 @@ else
git submodule update;
fi
if [ -d uavcan ]
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked uavcan submodule, correct version found"
else
echo ""
echo ""
echo "uavcan sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else
git submodule init;
git submodule update;
fi
exit 0

207
Tools/fetch_file.py Normal file
View File

@ -0,0 +1,207 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2013-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.
#
############################################################################
"""Fetch files via nsh console
Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path
\t-l\tList files
\t-f\tOverwrite existing files
\t-d\tSerial device
\t-s\tSerial baudrate
\t-o\tOutput path
\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively"""
__author__ = "Anton Babushkin"
__version__ = "1.1"
import serial, time, sys, os
def _wait_for_string(ser, s, timeout):
t0 = time.time()
buf = []
res = []
while (True):
c = ser.read()
buf.append(c)
if len(buf) > len(s):
res.append(buf.pop(0))
if "".join(buf) == s:
break
if timeout > 0.0 and time.time() - t0 > timeout:
raise Exception("Timeout while waiting for: " + s)
return "".join(res)
def _exec_cmd(ser, cmd, timeout):
ser.write(cmd + "\n")
ser.flush()
_wait_for_string(ser, cmd + "\r\n", timeout)
return _wait_for_string(ser, "nsh> \x1b[K", timeout)
def _ls_dir_raw(ser, dir, timeout):
return _exec_cmd(ser, "ls -l " + dir, timeout)
def _ls_dir(ser, dir, timeout):
res = []
for line in _ls_dir_raw(ser, dir, timeout).splitlines():
if line == dir + ":":
continue
if line.startswith("nsh: ls: no such directory:"):
raise Exception("No such file: " + dir)
res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
return res
def _get_file(ser, fn, fn_out, force, timeout):
print "Get %s:" % fn,
if not force:
# Check if file already exists with the same size
try:
os.stat(fn_out)
except:
pass
else:
print "already fetched, skip"
return
cmd = "dumpfile " + fn
ser.write(cmd + "\n")
ser.flush()
_wait_for_string(ser, cmd + "\r\n", timeout)
res = _wait_for_string(ser, "\n", timeout)
if res.startswith("OK"):
# Got correct responce, open temp file
fn_out_part = fn_out + ".part"
fout = open(fn_out_part, "wb")
size = int(res.split()[1])
sys.stdout.write(" [%i bytes] " % size)
n = 0
while (n < size):
buf = ser.read(min(size - n, 8192))
n += len(buf)
sys.stdout.write(".")
sys.stdout.flush()
fout.write(buf)
print " done"
fout.close()
os.rename(fn_out_part, fn_out)
else:
raise Exception("Error reading file")
_wait_for_string(ser, "nsh> \x1b[K", timeout)
def _get_files_in_dir(ser, path, path_out, force, timeout):
try:
os.mkdir(path_out)
except:
pass
for fn in _ls_dir(ser, path, timeout):
path_fn = os.path.join(path, fn[0])
path_fn_out = os.path.join(path_out, fn[0])
if fn[2]:
_get_files_in_dir(ser, path_fn[:-1], path_fn_out[:-1], force, timeout)
else:
_get_file(ser, path_fn, path_fn_out, force, timeout)
def _usage():
print """Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path
\t-l\tList files
\t-f\tOverwrite existing files
\t-d\tSerial device
\t-s\tSerial baudrate
\t-o\tOutput path
\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively"""
def _main():
dev = "/dev/tty.usbmodem1"
speed = "57600"
cmd = "get"
path = None
path_out = None
force = False
opt = None
for arg in sys.argv[1:]:
if opt != None:
if opt == "d":
dev = arg
elif opt == "s":
speed = arg
elif opt == "o":
path_out = arg
opt = None
else:
if arg == "-l":
cmd = "ls"
elif arg == "-f":
force = True
elif arg == "-d":
opt = "d"
elif arg == "-s":
opt = "s"
elif arg == "-o":
opt = "o"
elif path == None:
path = arg
if path == None:
_usage()
exit(0)
# Connect to serial port
ser = serial.Serial(dev, speed, timeout=0.2)
timeout = 1.0
try:
if cmd == "ls":
# List directory
print _ls_dir_raw(ser, path, timeout)
elif cmd == "get":
# Get file(s)
if path.endswith("/"):
# Get all files from directory recursively
if path_out == None:
path_out = os.path.split(path[:-1])[1]
_get_files_in_dir(ser, path[:-1], path_out, force, timeout)
else:
# Get one file
if path_out == None:
path_out = os.path.split(path)[1]
_get_file(ser, path, os.path.split(path)[1], force, timeout)
except Exception as e:
print e
ser.close()
if __name__ == "__main__":
_main()

133
Tools/fetch_log.py
View File

@ -1,133 +0,0 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2012, 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
# 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.
#
############################################################################
#
# Log fetcher
#
# Print list of logs:
# python fetch_log.py
#
# Fetch log:
# python fetch_log.py sess001/log001.bin
#
import serial, time, sys, os
def wait_for_string(ser, s, timeout=1.0, debug=False):
t0 = time.time()
buf = []
res = []
n = 0
while (True):
c = ser.read()
if debug:
sys.stderr.write(c)
buf.append(c)
if len(buf) > len(s):
res.append(buf.pop(0))
n += 1
if n % 10000 == 0:
sys.stderr.write(str(n) + "\n")
if "".join(buf) == s:
break
if timeout > 0.0 and time.time() - t0 > timeout:
raise Exception("Timeout while waiting for: " + s)
return "".join(res)
def exec_cmd(ser, cmd, timeout):
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout)
return wait_for_string(ser, "nsh> \x1b[K", timeout)
def ls_dir(ser, dir, timeout=1.0):
res = []
for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
return res
def list_logs(ser):
logs_dir = "/fs/microsd/log"
res = []
for d in ls_dir(ser, logs_dir):
if d[2]:
sess_dir = d[0][:-1]
for f in ls_dir(ser, logs_dir + "/" + sess_dir):
log_file = f[0]
log_size = f[1]
res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
return "\n".join(res)
def fetch_log(ser, fn, timeout):
cmd = "dumpfile " + fn
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout, True)
res = wait_for_string(ser, "\n", timeout, True)
data = []
if res.startswith("OK"):
size = int(res.split()[1])
n = 0
print "Reading data:"
while (n < size):
buf = ser.read(min(size - n, 8192))
data.append(buf)
n += len(buf)
sys.stdout.write(".")
sys.stdout.flush()
print
else:
raise Exception("Error reading log")
wait_for_string(ser, "nsh> \x1b[K", timeout)
return "".join(data)
def main():
dev = "/dev/tty.usbmodem1"
ser = serial.Serial(dev, "115200", timeout=0.2)
if len(sys.argv) < 2:
print list_logs(ser)
else:
log_file = sys.argv[1]
data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
try:
os.mkdir(log_file.split("/")[0])
except:
pass
fout = open(log_file, "wb")
fout.write(data)
fout.close()
ser.close()
if __name__ == "__main__":
main()

14
Tools/tests-host/data/fit_linear_voltage.m Normal file
View File

@ -0,0 +1,14 @@
close all;
clear all;
M = importdata('px4io_v1.3.csv');
voltage = M.data(:, 1);
counts = M.data(:, 2);
plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15);
coeffs = polyfit(counts, voltage, 1);
fittedC = linspace(min(counts), max(counts), 500);
fittedV = polyval(coeffs, fittedC);
hold on
plot(fittedC, fittedV, 'r-', 'LineWidth', 3);
slope = coeffs(1)
y_intersection = coeffs(2)

70
Tools/tests-host/data/px4io_v1.3.csv Normal file
View File

@ -0,0 +1,70 @@
voltage, counts
4.3, 950
4.4, 964
4.5, 986
4.6, 1009
4.7, 1032
4.8, 1055
4.9, 1078
5.0, 1101
5.2, 1124
5.3, 1148
5.4, 1171
5.5, 1195
6.0, 1304
6.1, 1329
6.2, 1352
7.0, 1517
7.1, 1540
7.2, 1564
7.3, 1585
7.4, 1610
7.5, 1636
8.0, 1728
8.1, 1752
8.2, 1755
8.3, 1798
8.4, 1821
9.0, 1963
9.1, 1987
9.3, 2010
9.4, 2033
10.0, 2174
10.1, 2198
10.2, 2221
10.3, 2245
10.4, 2268
11.0, 2385
11.1, 2409
11.2, 2432
11.3, 2456
11.4, 2480
11.5, 2502
11.6, 2526
11.7, 2550
11.8, 2573
11.9, 2597
12.0, 2621
12.1, 2644
12.3, 2668
12.4, 2692
12.5, 2716
12.6, 2737
12.7, 2761
13.0, 2832
13.5, 2950
13.6, 2973
14.1, 3068
14.2, 3091
14.7, 3209
15.0, 3280
15.1, 3304
15.5, 3374
15.6, 3397
15.7, 3420
16.0, 3492
16.1, 3514
16.2, 3538
16.9, 3680
17.0, 3704
17.1, 3728
1 voltage counts
2 4.3 950
3 4.4 964
4 4.5 986
5 4.6 1009
6 4.7 1032
7 4.8 1055
8 4.9 1078
9 5.0 1101
10 5.2 1124
11 5.3 1148
12 5.4 1171
13 5.5 1195
14 6.0 1304
15 6.1 1329
16 6.2 1352
17 7.0 1517
18 7.1 1540
19 7.2 1564
20 7.3 1585
21 7.4 1610
22 7.5 1636
23 8.0 1728
24 8.1 1752
25 8.2 1755
26 8.3 1798
27 8.4 1821
28 9.0 1963
29 9.1 1987
30 9.3 2010
31 9.4 2033
32 10.0 2174
33 10.1 2198
34 10.2 2221
35 10.3 2245
36 10.4 2268
37 11.0 2385
38 11.1 2409
39 11.2 2432
40 11.3 2456
41 11.4 2480
42 11.5 2502
43 11.6 2526
44 11.7 2550
45 11.8 2573
46 11.9 2597
47 12.0 2621
48 12.1 2644
49 12.3 2668
50 12.4 2692
51 12.5 2716
52 12.6 2737
53 12.7 2761
54 13.0 2832
55 13.5 2950
56 13.6 2973
57 14.1 3068
58 14.2 3091
59 14.7 3209
60 15.0 3280
61 15.1 3304
62 15.5 3374
63 15.6 3397
64 15.7 3420
65 16.0 3492
66 16.1 3514
67 16.2 3538
68 16.9 3680
69 17.0 3704
70 17.1 3728

1
makefiles/config_aerocore_default.mk
View File

@ -80,6 +80,7 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -108,6 +108,7 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion

2
makefiles/config_px4fmu-v2_default.mk
View File

@ -74,6 +74,7 @@ MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/gpio_led
MODULES += modules/uavcan
#
# Estimation modules (EKF/ SO3 / other filters)
@ -120,6 +121,7 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion

5
makefiles/config_px4fmu-v2_test.mk
View File

@ -33,6 +33,11 @@ MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/ver
#
# Testing modules
#
MODULES += examples/matlab_csv_serial
#
# Library modules
#

1
makefiles/nuttx.mk
View File

@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script
# Add directories from the NuttX export to the relevant search paths
#
INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
$(NUTTX_EXPORT_DIR)include/cxx \
$(NUTTX_EXPORT_DIR)arch/chip \
$(NUTTX_EXPORT_DIR)arch/common

1
makefiles/setup.mk
View File

@ -49,6 +49,7 @@ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/
export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/
export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/
export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/
export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/

2
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
# Generic warnings
#

2
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit d1ebe85eb6bb06d0078f3e0b8144adb131263628
Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9

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

@ -107,6 +107,10 @@ private:
RingBuffer *_reports;
perf_counter_t _buffer_overflows;
/* this class has pointer data members and should not be copied */
Airspeed(const Airspeed&);
Airspeed& operator=(const Airspeed&);
protected:
virtual int probe();

2
src/drivers/airspeed/module.mk
View File

@ -36,3 +36,5 @@
#
SRCS = airspeed.cpp
MAXOPTIMIZATION = -Os

2
src/drivers/bma180/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = bma180
SRCS = bma180.cpp
MAXOPTIMIZATION = -Os

11
src/drivers/device/device.h
View File

@ -240,6 +240,7 @@ private:
* @param context Pointer to the interrupted context.
*/
static void dev_interrupt(int irq, void *context);
};
/**
@ -469,6 +470,10 @@ private:
* @return OK, or -errno on error.
*/
int remove_poll_waiter(struct pollfd *fds);
/* do not allow copying this class */
CDev(const CDev&);
CDev operator=(const CDev&);
};
/**
@ -538,6 +543,10 @@ private:
} // namespace device
// class instance for primary driver of each class
#define CLASS_DEVICE_PRIMARY 0
enum CLASS_DEVICE {
CLASS_DEVICE_PRIMARY=0,
CLASS_DEVICE_SECONDARY=1,
CLASS_DEVICE_TERTIARY=2
};
#endif /* _DEVICE_DEVICE_H */

3
src/drivers/device/i2c.h
View File

@ -138,6 +138,9 @@ private:
uint16_t _address;
uint32_t _frequency;
struct i2c_dev_s *_dev;
I2C(const device::I2C&);
I2C operator=(const device::I2C&);
};
} // namespace device

4
src/drivers/device/ringbuffer.h
View File

@ -162,6 +162,10 @@ private:
volatile unsigned _tail; /**< removal point in _item_size units */
unsigned _next(unsigned index);
/* we don't want this class to be copied */
RingBuffer(const RingBuffer&);
RingBuffer operator=(const RingBuffer&);
};
RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :

5
src/drivers/device/spi.h
View File

@ -129,10 +129,15 @@ private:
uint32_t _frequency;
struct spi_dev_s *_dev;
/* this class does not allow copying */
SPI(const SPI&);
SPI operator=(const SPI&);
protected:
int _bus;
int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
};
} // namespace device

4
src/drivers/drv_accel.h
View File

@ -81,7 +81,9 @@ struct accel_scale {
/*
* ObjDev tag for raw accelerometer data.
*/
ORB_DECLARE(sensor_accel);
ORB_DECLARE(sensor_accel0);
ORB_DECLARE(sensor_accel1);
ORB_DECLARE(sensor_accel2);
/*
* ioctl() definitions

3
src/drivers/drv_baro.h
View File

@ -63,7 +63,8 @@ struct baro_report {
/*
* ObjDev tag for raw barometer data.
*/
ORB_DECLARE(sensor_baro);
ORB_DECLARE(sensor_baro0);
ORB_DECLARE(sensor_baro1);
/*
* ioctl() definitions

4
src/drivers/drv_gyro.h
View File

@ -81,7 +81,9 @@ struct gyro_scale {
/*
* ObjDev tag for raw gyro data.
*/
ORB_DECLARE(sensor_gyro);
ORB_DECLARE(sensor_gyro0);
ORB_DECLARE(sensor_gyro1);
ORB_DECLARE(sensor_gyro2);
/*
* ioctl() definitions

5
src/drivers/drv_mag.h
View File

@ -79,8 +79,9 @@ struct mag_scale {
/*
* ObjDev tag for raw magnetometer data.
*/
ORB_DECLARE(sensor_mag);
ORB_DECLARE(sensor_mag0);
ORB_DECLARE(sensor_mag1);
ORB_DECLARE(sensor_mag2);
/*
* mag device types, for _device_id

3
src/drivers/drv_pwm_output.h
View File

@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
/*
*
*

1
src/drivers/drv_tone_alarm.h
View File

@ -149,6 +149,7 @@ enum {
TONE_GPS_WARNING_TUNE,
TONE_ARMING_FAILURE_TUNE,
TONE_PARACHUTE_RELEASE_TUNE,
TONE_EKF_WARNING_TUNE,
TONE_NUMBER_OF_TUNES
};

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

@ -286,6 +286,9 @@ void info();
/**
* Start the driver.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
void
start(int i2c_bus)

5
src/drivers/ets_airspeed/module.mk
View File

@ -36,6 +36,9 @@
#
MODULE_COMMAND = ets_airspeed
MODULE_STACKSIZE = 2048
SRCS = ets_airspeed.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

2
src/drivers/hil/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = hil
SRCS = hil.cpp
MAXOPTIMIZATION = -Os

48
src/drivers/hmc5883/hmc5883.cpp
View File

@ -162,6 +162,7 @@ private:
orb_advert_t _mag_topic;
orb_advert_t _subsystem_pub;
orb_id_t _mag_orb_id;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@ -337,6 +338,9 @@ private:
*/
int check_offset();
/* this class has pointer data members, do not allow copying it */
HMC5883(const HMC5883&);
HMC5883 operator=(const HMC5883&);
};
/*
@ -347,14 +351,17 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
_work{},
_measure_ticks(0),
_reports(nullptr),
_scale{},
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_collect_phase(false),
_class_instance(-1),
_mag_topic(-1),
_subsystem_pub(-1),
_mag_orb_id(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@ -423,6 +430,20 @@ HMC5883::init()
_class_instance = register_class_devname(MAG_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_mag_orb_id = ORB_ID(sensor_mag0);
break;
case CLASS_DEVICE_SECONDARY:
_mag_orb_id = ORB_ID(sensor_mag1);
break;
case CLASS_DEVICE_TERTIARY:
_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
@ -867,7 +888,8 @@ HMC5883::collect()
struct {
int16_t x, y, z;
} report;
int ret = -EIO;
int ret;
uint8_t cmd;
uint8_t check_counter;
@ -945,16 +967,16 @@ HMC5883::collect()
// apply user specified rotation
rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (!(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
orb_publish(_mag_orb_id, _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
_mag_topic = orb_advertise(_mag_orb_id, &new_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
debug("ADVERT FAIL");
}
}
@ -1157,7 +1179,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
warn("failed to set new scale / offsets for mag");
}
/* set back to normal mode */
@ -1341,8 +1363,8 @@ namespace hmc5883
#endif
const int ERROR = -1;
HMC5883 *g_dev_int;
HMC5883 *g_dev_ext;
HMC5883 *g_dev_int = nullptr;
HMC5883 *g_dev_ext = nullptr;
void start(int bus, enum Rotation rotation);
void test(int bus);
@ -1353,6 +1375,9 @@ void usage();
/**
* Start the driver.
*
* This function call only returns once the driver
* is either successfully up and running or failed to start.
*/
void
start(int bus, enum Rotation rotation)
@ -1378,6 +1403,11 @@ start(int bus, enum Rotation rotation)
errx(0, "already started internal");
g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
if (bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
@ -1443,7 +1473,7 @@ test(int bus)
int fd = open(path, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
err(1, "%s open failed (try 'hmc5883 start')", path);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));

7
src/drivers/hmc5883/module.mk
View File

@ -37,7 +37,8 @@
MODULE_COMMAND = hmc5883
# XXX seems excessive, check if 2048 is sufficient
MODULE_STACKSIZE = 4096
SRCS = hmc5883.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

44
src/drivers/l3gd20/l3gd20.cpp
View File

@ -213,6 +213,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
orb_id_t _orb_id;
int _class_instance;
unsigned _current_rate;
@ -330,15 +331,22 @@ private:
* @return 0 on success, 1 on failure
*/
int self_test();
/* this class does not allow copying */
L3GD20(const L3GD20&);
L3GD20 operator=(const L3GD20&);
};
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call{},
_call_interval(0),
_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_orb_id(nullptr),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@ -399,21 +407,32 @@ L3GD20::init()
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_orb_id = ORB_ID(sensor_gyro2);
break;
}
reset();
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
_gyro_topic = orb_advertise(_orb_id, &grp);
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
}
ret = OK;
@ -931,9 +950,9 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
orb_publish(_orb_id, _gyro_topic, &report);
}
_read++;
@ -990,6 +1009,9 @@ void info();
/**
* Start the driver.
*
* This function call only returns once the driver
* started or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation)

4
src/drivers/l3gd20/module.mk
View File

@ -4,3 +4,7 @@
MODULE_COMMAND = l3gd20
SRCS = l3gd20.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

2
src/drivers/led/module.mk
View File

@ -36,3 +36,5 @@
#
SRCS = led.cpp
MAXOPTIMIZATION = -Os

82
src/drivers/lsm303d/lsm303d.cpp
View File

@ -284,6 +284,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_class_instance;
unsigned _accel_read;
@ -461,6 +462,10 @@ private:
* @return OK if the value can be supported.
*/
int mag_set_samplerate(unsigned frequency);
/* this class cannot be copied */
LSM303D(const LSM303D&);
LSM303D operator=(const LSM303D&);
};
/**
@ -485,29 +490,39 @@ private:
LSM303D *_parent;
orb_advert_t _mag_topic;
orb_id_t _mag_orb_id;
int _mag_class_instance;
void measure();
void measure_trampoline(void *arg);
/* this class does not allow copying due to ptr data members */
LSM303D_mag(const LSM303D_mag&);
LSM303D_mag operator=(const LSM303D_mag&);
};
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
_mag(new LSM303D_mag(this)),
_accel_call{},
_mag_call{},
_call_accel_interval(0),
_call_mag_interval(0),
_accel_reports(nullptr),
_mag_reports(nullptr),
_accel_scale{},
_accel_range_m_s2(0.0f),
_accel_range_scale(0.0f),
_accel_samplerate(0),
_accel_onchip_filter_bandwith(0),
_mag_scale{},
_mag_range_ga(0.0f),
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
@ -524,6 +539,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_reg7_expected(0),
_accel_log_fd(-1),
_accel_logging_enabled(false),
_last_extreme_us(0),
_last_log_us(0),
_last_log_sync_us(0),
_last_log_reg_us(0),
@ -611,34 +627,56 @@ LSM303D::init()
/* fill report structures */
measure();
if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
switch (_mag->_mag_class_instance) {
case CLASS_DEVICE_PRIMARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag0);
break;
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
case CLASS_DEVICE_SECONDARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag1);
break;
if (_mag->_mag_topic < 0)
debug("failed to create sensor_mag publication");
case CLASS_DEVICE_TERTIARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
if (_mag->_mag_topic < 0) {
warnx("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
if (_accel_topic < 0) {
warnx("ADVERT ERR");
}
out:
@ -1557,9 +1595,9 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
orb_publish(_accel_orb_id, _accel_topic, &accel_report);
}
_accel_read++;
@ -1634,9 +1672,9 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
}
_mag_read++;
@ -1730,6 +1768,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_orb_id(nullptr),
_mag_class_instance(-1)
{
}
@ -1803,6 +1842,9 @@ void usage();
/**
* Start the driver.
*
* This function call only returns once the driver is
* up and running or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation)

2
src/drivers/lsm303d/module.mk
View File

@ -5,4 +5,6 @@
MODULE_COMMAND = lsm303d
SRCS = lsm303d.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

2
src/drivers/md25/module.mk
View File

@ -40,3 +40,5 @@ MODULE_COMMAND = md25
SRCS = md25.cpp \
md25_main.cpp
MAXOPTIMIZATION = -Os

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

@ -92,7 +92,7 @@
#include <drivers/airspeed/airspeed.h>
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */
#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */
#define PATH_MS4525 "/dev/ms4525"
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
@ -102,9 +102,9 @@
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
/* Measurement rate is 100Hz */
#define MEAS_RATE 100.0f
#define MEAS_DRIVER_FILTER_FREQ 3.0f
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
#define MEAS_RATE 100
#define MEAS_DRIVER_FILTER_FREQ 1.5f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeed : public Airspeed
{
@ -140,9 +140,9 @@ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1)
_t_system_power(-1),
system_power{}
{
memset(&system_power, 0, sizeof(system_power));
}
int
@ -420,6 +420,9 @@ void info();
/**
* Start the driver.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
*/
void
start(int i2c_bus)

7
src/drivers/meas_airspeed/module.mk
View File

@ -36,6 +36,11 @@
#
MODULE_COMMAND = meas_airspeed
MODULE_STACKSIZE = 2048
SRCS = meas_airspeed.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os

7
src/drivers/mpu6000/module.mk
View File

@ -37,7 +37,8 @@
MODULE_COMMAND = mpu6000
# XXX seems excessive, check if 2048 is not sufficient
MODULE_STACKSIZE = 4096
SRCS = mpu6000.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

83
src/drivers/mpu6000/mpu6000.cpp
View File

@ -215,6 +215,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_class_instance;
RingBuffer *_gyro_reports;
@ -343,6 +344,9 @@ private:
*/
void _set_sample_rate(uint16_t desired_sample_rate_hz);
/* do not allow to copy this class due to pointer data members */
MPU6000(const MPU6000&);
MPU6000 operator=(const MPU6000&);
};
/**
@ -367,8 +371,12 @@ protected:
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
orb_id_t _gyro_orb_id;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
MPU6000_gyro(const MPU6000_gyro&);
MPU6000_gyro operator=(const MPU6000_gyro&);
};
/** driver 'main' command */
@ -378,13 +386,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
_call{},
_call_interval(0),
_accel_reports(nullptr),
_accel_scale{},
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_sample_rate(1000),
@ -498,31 +510,56 @@ MPU6000::init()
measure();
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
_accel_topic = orb_advertise(_accel_orb_id, &arp);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
if (_accel_topic < 0) {
warnx("ADVERT FAIL");
}
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro->_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
switch (_gyro->_gyro_class_instance) {
case CLASS_DEVICE_PRIMARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
break;
}
_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
if (_gyro->_gyro_topic < 0) {
warnx("ADVERT FAIL");
}
out:
@ -1345,14 +1382,14 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (_accel_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
orb_publish(_accel_orb_id, _accel_topic, &arb);
}
if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@ -1373,6 +1410,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_orb_id(nullptr),
_gyro_class_instance(-1)
{
}
@ -1437,6 +1475,9 @@ void usage();
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation)
@ -1507,7 +1548,7 @@ test(bool external_bus)
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
err(1, "%s open failed (try 'mpu6000 start')",
path_accel);
/* get the driver */

2
src/drivers/ms5611/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = ms5611
SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
MAXOPTIMIZATION = -Os

27
src/drivers/ms5611/ms5611.cpp
View File

@ -300,12 +300,17 @@ MS5611::init()
ret = OK;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp);
break;
case CLASS_DEVICE_SECONDARY:
_baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp);
break;
}
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
if (_baro_topic < 0)
debug("failed to create sensor_baro publication");
if (_baro_topic < 0) {
warnx("failed to create sensor_baro publication");
}
} while (0);
@ -722,9 +727,17 @@ MS5611::collect()
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
if (_baro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report);
break;
case CLASS_DEVICE_SECONDARY:
orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report);
break;
}
}
if (_reports->force(&report)) {

2
src/drivers/pca8574/module.mk
View File

@ -4,3 +4,5 @@
MODULE_COMMAND = pca8574
SRCS = pca8574.cpp
MAXOPTIMIZATION = -Os

2
src/drivers/px4flow/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = px4flow
SRCS = px4flow.cpp
MAXOPTIMIZATION = -Os

5
src/drivers/px4fmu/fmu.cpp
View File

@ -179,6 +179,9 @@ private:
uint32_t gpio_read(void);
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
/* do not allow to copy due to ptr data members */
PX4FMU(const PX4FMU&);
PX4FMU operator=(const PX4FMU&);
};
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@ -242,6 +245,7 @@ PX4FMU::PX4FMU() :
_task(-1),
_armed_sub(-1),
_outputs_pub(-1),
_armed{},
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@ -252,6 +256,7 @@ PX4FMU::PX4FMU() :
_groups_subscribed(0),
_control_subs{-1},
_poll_fds_num(0),
_pwm_limit{},
_failsafe_pwm{0},
_disarmed_pwm{0},
_num_failsafe_set(0),

2
src/drivers/px4fmu/module.mk
View File

@ -6,3 +6,5 @@ MODULE_COMMAND = fmu
SRCS = fmu.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

2
src/drivers/px4io/module.mk
View File

@ -46,3 +46,5 @@ SRCS = px4io.cpp \
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++

42
src/drivers/px4io/px4io.cpp
View File

@ -453,6 +453,9 @@ private:
*/
void io_handle_vservo(uint16_t vservo, uint16_t vrssi);
/* do not allow to copy this class due to ptr data members */
PX4IO(const PX4IO&);
PX4IO operator=(const PX4IO&);
};
namespace
@ -496,6 +499,8 @@ PX4IO::PX4IO(device::Device *interface) :
_to_battery(0),
_to_servorail(0),
_to_safety(0),
_outputs{},
_servorail_status{},
_primary_pwm_device(false),
_lockdown_override(false),
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
@ -1144,6 +1149,12 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
if (armed.force_failsafe) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@ -1997,7 +2008,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@ -2005,7 +2016,9 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""));
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@ -2217,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
/* clear force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
} else {
/* set force failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@ -2419,7 +2443,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
case PX4IO_CHECK_CRC: {
/* check IO firmware CRC against passed value */
/* check IO firmware CRC against passed value */
uint32_t io_crc = 0;
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
if (ret != OK)
@ -2679,7 +2703,7 @@ checkcrc(int argc, char *argv[])
int fd = open(argv[1], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[1], errno);
exit(1);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
@ -2694,7 +2718,7 @@ checkcrc(int argc, char *argv[])
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
@ -2707,7 +2731,7 @@ checkcrc(int argc, char *argv[])
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
exit(1);
}
printf("CRCs match\n");
exit(0);
@ -2737,12 +2761,12 @@ bind(int argc, char *argv[])
pulses = DSMX_BIND_PULSES;
else if (!strcmp(argv[2], "dsmx8"))
pulses = DSMX8_BIND_PULSES;
else
else
errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
// Test for custom pulse parameter
if (argc > 3)
pulses = atoi(argv[3]);
if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
errx(1, "system must not be armed");
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
@ -2944,7 +2968,7 @@ lockdown(int argc, char *argv[])
(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
warnx("ACTUATORS ARE NOW SAFE IN HIL.");
}
} else {
errx(1, "driver not loaded, exiting");
}

8
src/drivers/px4io/px4io_serial.cpp
View File

@ -157,6 +157,10 @@ private:
perf_counter_t _pc_idle;
perf_counter_t _pc_badidle;
/* do not allow top copying this class */
PX4IO_serial(PX4IO_serial &);
PX4IO_serial& operator = (const PX4IO_serial &);
};
IOPacket PX4IO_serial::_dma_buffer;
@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() :
_tx_dma(nullptr),
_rx_dma(nullptr),
_rx_dma_status(_dma_status_inactive),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_bus_semaphore(SEM_INITIALIZER(0)),
_completion_semaphore(SEM_INITIALIZER(0)),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),

76
src/drivers/px4io/px4io_uploader.cpp
View File

@ -65,7 +65,8 @@
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
_fw_fd(-1),
bl_rev(0)
{
}
@ -204,12 +205,8 @@ PX4IO_Uploader::upload(const char *filenames[])
if (bl_rev <= 2) {
ret = verify_rev2(fw_size);
} else if(bl_rev == 3) {
ret = verify_rev3(fw_size);
} else {
/* verify rev 4 and higher still uses the same approach and
* every version *needs* to be verified.
*/
/* verify rev 3 and higher. Every version *needs* to be verified. */
ret = verify_rev3(fw_size);
}
@ -249,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
}
int
PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout)
{
struct pollfd fds[1];
@ -266,24 +263,24 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
return -ETIMEDOUT;
}
read(_io_fd, &c, 1);
read(_io_fd, c, 1);
#ifdef UDEBUG
log("recv 0x%02x", c);
log("recv_bytes 0x%02x", c);
#endif
return OK;
}
int
PX4IO_Uploader::recv(uint8_t *p, unsigned count)
PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
{
int ret = OK;
while (count--) {
int ret = recv(*p++, 5000);
ret = recv_byte_with_timeout(p++, 5000);
if (ret != OK)
return ret;
break;
}
return OK;
return ret;
}
void
@ -293,10 +290,10 @@ PX4IO_Uploader::drain()
int ret;
do {
// the small recv timeout here is to allow for fast
// the small recv_bytes timeout here is to allow for fast
// drain when rebooting the io board for a forced
// update of the fw without using the safety switch
ret = recv(c, 40);
ret = recv_byte_with_timeout(&c, 40);
#ifdef UDEBUG
if (ret == OK) {
@ -314,21 +311,19 @@ PX4IO_Uploader::send(uint8_t c)
#endif
if (write(_io_fd, &c, 1) != 1)
return -errno;
return OK;
}
int
PX4IO_Uploader::send(uint8_t *p, unsigned count)
{
int ret;
while (count--) {
int ret = send(*p++);
ret = send(*p++);
if (ret != OK)
return ret;
break;
}
return OK;
return ret;
}
int
@ -337,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout)
uint8_t c[2];
int ret;
ret = recv(c[0], timeout);
ret = recv_byte_with_timeout(c, timeout);
if (ret != OK)
return ret;
ret = recv(c[1], timeout);
ret = recv_byte_with_timeout(c + 1, timeout);
if (ret != OK)
return ret;
@ -378,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
send(param);
send(PROTO_EOC);
ret = recv((uint8_t *)&val, sizeof(val));
ret = recv_bytes((uint8_t *)&val, sizeof(val));
if (ret != OK)
return ret;
@ -419,12 +414,15 @@ PX4IO_Uploader::program(size_t fw_size)
int ret;
size_t sent = 0;
file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
file_buf = new uint8_t[PROG_MULTI_MAX];
if (!file_buf) {
log("Can't allocate program buffer");
return -ENOMEM;
}
ASSERT((fw_size & 3) == 0);
ASSERT((PROG_MULTI_MAX & 3) == 0);
log("programming %u bytes...", (unsigned)fw_size);
ret = lseek(_fw_fd, 0, SEEK_SET);
@ -443,34 +441,26 @@ PX4IO_Uploader::program(size_t fw_size)
(unsigned)sent,
(int)count,
(int)errno);
}
if (count == 0) {
free(file_buf);
return OK;
ret = -errno;
break;
}
sent += count;
if (count < 0)
return -errno;
ASSERT((count % 4) == 0);
send(PROTO_PROG_MULTI);
send(count);
send(&file_buf[0], count);
send(file_buf, count);
send(PROTO_EOC);
ret = get_sync(1000);
if (ret != OK) {
free(file_buf);
return ret;
break;
}
}
free(file_buf);
return OK;
delete [] file_buf;
return ret;
}
int
@ -524,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
for (ssize_t i = 0; i < count; i++) {
uint8_t c;
ret = recv(c, 5000);
ret = recv_byte_with_timeout(&c, 5000);
if (ret != OK) {
log("%d: got %d waiting for bytes", sent + i, ret);
@ -611,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
send(PROTO_GET_CRC);
send(PROTO_EOC);
ret = recv((uint8_t*)(&crc), sizeof(crc));
ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
if (ret != OK) {
log("did not receive CRC checksum");

9
src/drivers/px4io/uploader.h
View File

@ -74,20 +74,19 @@ private:
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */
};
int _io_fd;
int _fw_fd;
uint32_t bl_rev; /**< bootloader revision */
uint32_t bl_rev; /**< bootloader revision */
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout);
int recv(uint8_t *p, unsigned count);
int recv_byte_with_timeout(uint8_t *c, unsigned timeout);
int recv_bytes(uint8_t *p, unsigned count);
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);

2
src/drivers/rgbled/module.mk
View File

@ -4,3 +4,5 @@
MODULE_COMMAND = rgbled
SRCS = rgbled.cpp
MAXOPTIMIZATION = -Os

2
src/drivers/roboclaw/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = roboclaw
SRCS = roboclaw_main.cpp \
RoboClaw.cpp
MAXOPTIMIZATION = -Os

2
src/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

@ -336,6 +336,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow
_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@ -348,6 +349,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning
_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm
_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
}
ToneAlarm::~ToneAlarm()

247
src/examples/matlab_csv_serial/matlab_csv_serial.c Normal file
View File

@ -0,0 +1,247 @@
/****************************************************************************
*
* 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 matlab_csv_serial_main.c
*
* Matlab CSV / ASCII format interface at 921600 baud, 8 data bits,
* 1 stop bit, no parity
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <poll.h>
__EXPORT int matlab_csv_serial_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
int matlab_csv_serial_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn_cmd().
*/
int matlab_csv_serial_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
warnx("already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = task_spawn_cmd("matlab_csv_serial",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop"))
{
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status"))
{
if (thread_running) {
warnx("running");
} else {
warnx("stopped");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int matlab_csv_serial_thread_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "need a serial port name as argument");
}
const char* uart_name = argv[1];
warnx("opening port %s", uart_name);
int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
unsigned speed = 921600;
if (serial_fd < 0) {
err(1, "failed to open port: %s", uart_name);
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
close(serial_fd);
return -1;
}
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* USB serial is indicated by /dev/ttyACM0*/
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
close(serial_fd);
return -1;
}
}
if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
close(serial_fd);
return -1;
}
/* subscribe to vehicle status, attitude, sensors and flow*/
struct accel_report accel0;
struct accel_report accel1;
struct gyro_report gyro0;
struct gyro_report gyro1;
/* subscribe to parameter changes */
int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
thread_running = true;
while (!thread_should_exit)
{
/*This runs at the rate of the sensors */
struct pollfd fds[] = {
{ .fd = accel0_sub, .events = POLLIN }
};
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
if (ret < 0)
{
/* poll error, ignore */
}
else if (ret == 0)
{
/* no return value, ignore */
warnx("no sensor data");
}
else
{
/* accel0 update available? */
if (fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
// write out on accel 0, but collect for all other sensors as they have updates
dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,
(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
}
}
}
warnx("exiting");
thread_running = false;
fflush(stdout);
return 0;
}

40
src/examples/matlab_csv_serial/module.mk Normal file
View File

@ -0,0 +1,40 @@
############################################################################
#
# 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.
#
############################################################################
#
# Build position estimator
#
MODULE_COMMAND = matlab_csv_serial
SRCS = matlab_csv_serial.c

2
src/lib/ecl/module.mk
View File

@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_controller.cpp
MAXOPTIMIZATION = -Os

2
src/lib/external_lgpl/module.mk
View File

@ -46,3 +46,5 @@
#
SRCS = tecs/tecs.cpp
MAXOPTIMIZATION = -Os

25
src/lib/mathlib/math/Matrix.hpp
View File

@ -69,27 +69,34 @@ public:
/**
* trivial ctor
* note that this ctor will not initialize elements
* Initializes the elements to zero.
*/
MatrixBase() {
arm_mat = {M, N, &data[0][0]};
MatrixBase() :
data{},
arm_mat{M, N, &data[0][0]}
{
}
virtual ~MatrixBase() {};
/**
* copyt ctor
*/
MatrixBase(const MatrixBase<M, N> &m) {
arm_mat = {M, N, &data[0][0]};
MatrixBase(const MatrixBase<M, N> &m) :
arm_mat{M, N, &data[0][0]}
{
memcpy(data, m.data, sizeof(data));
}
MatrixBase(const float *d) {
arm_mat = {M, N, &data[0][0]};
MatrixBase(const float *d) :
arm_mat{M, N, &data[0][0]}
{
memcpy(data, d, sizeof(data));
}
MatrixBase(const float d[M][N]) {
arm_mat = {M, N, &data[0][0]};
MatrixBase(const float d[M][N]) :
arm_mat{M, N, &data[0][0]}
{
memcpy(data, d, sizeof(data));
}

21
src/lib/mathlib/math/Vector.hpp
View File

@ -69,25 +69,32 @@ public:
/**
* trivial ctor
* note that this ctor will not initialize elements
* initializes elements to zero
*/
VectorBase() {
arm_col = {N, 1, &data[0]};
VectorBase() :
data{},
arm_col{N, 1, &data[0]}
{
}
virtual ~VectorBase() {};
/**
* copy ctor
*/
VectorBase(const VectorBase<N> &v) {
arm_col = {N, 1, &data[0]};
VectorBase(const VectorBase<N> &v) :
arm_col{N, 1, &data[0]}
{
memcpy(data, v.data, sizeof(data));
}
/**
* setting ctor
*/
VectorBase(const float d[N]) {
arm_col = {N, 1, &data[0]};
VectorBase(const float d[N]) :
arm_col{N, 1, &data[0]}
{
memcpy(data, d, sizeof(data));
}

12
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -46,10 +46,18 @@ class __EXPORT LowPassFilter2p
{
public:
// constructor
LowPassFilter2p(float sample_freq, float cutoff_freq) {
LowPassFilter2p(float sample_freq, float cutoff_freq) :
_cutoff_freq(cutoff_freq),
_a1(0.0f),
_a2(0.0f),
_b0(0.0f),
_b1(0.0f),
_b2(0.0f),
_delay_element_1(0.0f),
_delay_element_2(0.0f)
{
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
_delay_element_1 = _delay_element_2 = 0;
}
/**

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

@ -186,7 +186,7 @@ int do_airspeed_calibration(int mavlink_fd)
usleep(500 * 1000);
calibration_counter = 0;
const int maxcount = 3000;
const unsigned maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {

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

@ -124,7 +124,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
@ -163,7 +163,8 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static float eph_epv_threshold = 5.0f;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@ -335,6 +336,7 @@ void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
/* read all relevant states */
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -546,24 +548,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
} else {
/* reject parachute depoyment not armed */
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@ -741,6 +738,7 @@ int commander_thread_main(int argc, char *argv[])
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
status.circuit_breaker_engaged_airspd_check = false;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@ -918,6 +916,11 @@ int commander_thread_main(int argc, char *argv[])
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
/* Subscribe to actuator controls (outputs) */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
control_status_leds(&status, &armed, true);
/* now initialized */
@ -977,6 +980,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status_changed = true;
@ -1106,32 +1110,32 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
bool eph_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
if (global_position.eph > eph_threshold * 2.5f) {
eph_good = false;
} else {
eph_epv_good = true;
eph_good = true;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
if (global_position.eph < eph_threshold) {
eph_good = true;
} else {
eph_epv_good = false;
eph_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@ -1161,8 +1165,8 @@ int commander_thread_main(int argc, char *argv[])
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
if (status.condition_local_position_valid) {
if (local_position.eph > eph_threshold * 2.5f) {
local_eph_good = false;
} else {
@ -1170,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
if (local_position.eph < eph_epv_threshold) {
if (local_position.eph < eph_threshold) {
local_eph_good = true;
} else {
@ -1199,13 +1203,17 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
}
}
@ -1267,13 +1275,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@ -1412,8 +1420,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "arming state transition denied");
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}
@ -1441,7 +1453,7 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */
if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
@ -1494,7 +1506,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;

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

@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
{
float ret = 0;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static param_t bat_v_load_drop_h;
static float bat_v_empty = 3.4f;
static float bat_v_full = 4.2f;
static float bat_v_load_drop = 0.06f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (!initialized) {
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_v_full_h = param_find("BAT_V_CHARGED");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
initialized = true;
}
if (counter % 100 == 0) {
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_v_load_drop_h, &bat_v_load_drop);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */

3
src/modules/commander/commander_helper.h
View File

@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
*
* @param voltage the current battery voltage
* @param discharged the discharged capacity
* @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage, float discharged);
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
#endif /* COMMANDER_HELPER_H_ */

12
src/modules/commander/commander_params.c
View File

@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
/**
* Voltage drop per cell on 100% load
*
* This implicitely defines the internal resistance
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
/**
* Number of cells.

4
src/modules/commander/gyro_calibration.cpp
View File

@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd)
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
struct gyro_report gyro_report;
while (calibration_counter < calibration_count) {
@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;

4
src/modules/commander/mag_calibration.cpp
View File

@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
struct mag_report mag;
/* limit update rate to get equally spaced measurements over time (in ms) */
@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;

30
src/modules/commander/state_machine_helper.cpp
View File

@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
}
}
@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
}
@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "INVAL: %s - %s";
const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
/* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
}
return ret;
@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
mavlink_log_critical(mavlink_fd, "hold still while arming");
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
/* this is frickin' fatal */
failed = true;
goto system_eval;
@ -661,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
goto system_eval;
}
if (!status->is_rotary_wing) {
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = orb_subscribe(ORB_ID(airspeed));

5
src/modules/controllib/block/Block.hpp
View File

@ -93,6 +93,11 @@ protected:
List<uORB::SubscriptionBase *> _subscriptions;
List<uORB::PublicationBase *> _publications;
List<BlockParamBase *> _params;
private:
/* this class has pointer data members and should not be copied (private constructor) */
Block(const control::Block&);
Block operator=(const control::Block&);
};
class __EXPORT SuperBlock :

16
src/modules/controllib/blocks.cpp
View File

@ -121,6 +121,9 @@ int blockLimitSymTest()
float BlockLowPass::update(float input)
{
if (!isfinite(getState())) {
setState(input);
}
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
@ -293,7 +296,18 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}

19
src/modules/controllib/blocks.hpp
View File

@ -114,7 +114,7 @@ public:
// methods
BlockLowPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_state(0),
_state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockLowPass() {};
@ -238,9 +238,25 @@ public:
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_initialized(false),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
/**
* Update the state and get current derivative
*
* This call updates the state and gets the current
* derivative. As the derivative is only valid
* on the second call to update, it will return
* no change (0) on the first. To get a closer
* estimate of the derivative on the first call,
* call setU() one time step before using the
* return value of update().
*
* @param input the variable to calculate the derivative of
* @return the current derivative
*/
float update(float input);
// accessors
void setU(float u) {_u = u;}
@ -249,6 +265,7 @@ public:
protected:
// attributes
float _u; /**< previous input */
bool _initialized;
BlockLowPass _lowPass; /**< low pass filter */
};

46
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -112,6 +112,10 @@ public:
*/
FixedwingEstimator();
/* we do not want people ever copying this class */
FixedwingEstimator(const FixedwingEstimator& that) = delete;
FixedwingEstimator operator=(const FixedwingEstimator&) = delete;
/**
* Destructor, also kills the sensors task.
*/
@ -371,9 +375,10 @@ FixedwingEstimator::FixedwingEstimator() :
_mag_offsets({}),
#ifdef SENSOR_COMBINED_SUB
_sensor_combined({}),
_sensor_combined{},
#endif
_pos_ref{},
_baro_ref(0.0f),
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),
@ -390,12 +395,18 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_baro_init(false),
_gps_initialized(false),
_gps_start_time(0),
_filter_start_time(0),
_last_sensor_timestamp(0),
_last_run(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_ekf_logging(true),
_debug(0),
_mavlink_fd(-1),
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
@ -619,10 +630,10 @@ FixedwingEstimator::check_filter_state()
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
// rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
// rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
// rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
@ -693,7 +704,7 @@ FixedwingEstimator::task_main()
/*
* do subscriptions
*/
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -1041,7 +1052,7 @@ FixedwingEstimator::task_main()
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude;
@ -1133,7 +1144,7 @@ FixedwingEstimator::task_main()
initVelNED[2] = _gps.vel_d_m_s;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
_baro_gps_offset = _baro.altitude - gps_alt;
_ekf->baroHgt = _baro.altitude;
@ -1454,25 +1465,6 @@ FixedwingEstimator::task_main()
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = 0.0f; // XXX get form filter
_wind.covariance_east = 0.0f;
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];

6
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -2773,7 +2773,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
ResetHeight();
ResetStoredStates();
ret = 3;
ret = 0;
}
// Reset the filter if gyro offsets are excessive
@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
current_ekf_state.states[i] = states[i];
}
current_ekf_state.n_states = n_states;
current_ekf_state.onGround = onGround;
current_ekf_state.staticMode = staticMode;
current_ekf_state.useCompass = useCompass;
current_ekf_state.useAirspeed = useAirspeed;
memcpy(err, &current_ekf_state, sizeof(*err));

7
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
View File

@ -45,8 +45,11 @@ void Vector3f::zero(void)
z = 0.0f;
}
Mat3f::Mat3f() {
identity();
Mat3f::Mat3f() :
x{1.0f, 0.0f, 0.0f},
y{0.0f, 1.0f, 0.0f},
z{0.0f, 0.0f, 1.0f}
{
}
void Mat3f::identity() {

4
src/modules/ekf_att_pos_estimator/estimator_utilities.h
View File

@ -68,6 +68,10 @@ struct ekf_status_report {
bool posTimeout;
bool hgtTimeout;
bool imuTimeout;
bool onGround;
bool staticMode;
bool useCompass;
bool useAirspeed;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;

2
src/modules/ekf_att_pos_estimator/module.mk
View File

@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
ekf_att_pos_estimator_params.c \
estimator_23states.cpp \
estimator_utilities.cpp
EXTRACXXFLAGS = -Weffc++

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

@ -529,7 +529,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
}
}
@ -577,7 +577,7 @@ FixedwingAttitudeControl::task_main()
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));

248
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -42,8 +42,8 @@
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Implementation for total energy control class:
* Thomas Gubler
* Original implementation for total energy control class:
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
*
* More details and acknowledgements in the referenced library headers.
*
@ -87,10 +87,13 @@
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
static int _control_task = -1; /**< task handle for sensor task */
/**
* L1 control app start / stop handling function
@ -115,9 +118,9 @@ public:
/**
* Start the sensors task.
*
* @return OK on success.
* @return OK on success.
*/
int start();
static int start();
/**
* Task status
@ -131,12 +134,10 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle for sensor task */
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@ -160,18 +161,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
/** manual control states */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
double _launch_lat;
double _launch_lon;
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn_horizontal;
@ -188,8 +177,8 @@ private:
/* Landingslope object */
Landingslope landingslope;
float flare_curve_alt_rel_last;
/* heading hold */
float target_bearing;
@ -205,6 +194,7 @@ private:
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
@ -216,6 +206,8 @@ private:
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float heightrate_p;
float speedrate_p;
float throttle_damp;
float integrator_gain;
float vertical_accel_limit;
@ -238,9 +230,6 @@ private:
float throttle_land_max;
float heightrate_p;
float speedrate_p;
float land_slope_angle;
float land_H1_virt;
float land_flare_alt_relative;
@ -259,6 +248,8 @@ private:
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t heightrate_p;
param_t speedrate_p;
param_t throttle_damp;
param_t integrator_gain;
param_t vertical_accel_limit;
@ -281,9 +272,6 @@ private:
param_t throttle_land_max;
param_t heightrate_p;
param_t speedrate_p;
param_t land_slope_angle;
param_t land_H1_virt;
param_t land_flare_alt_relative;
@ -406,7 +394,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false),
_task_running(false),
_control_task(-1),
/* subscriptions */
_global_pos_sub(-1),
@ -416,12 +403,14 @@ FixedwingPositionControl::FixedwingPositionControl() :
_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
_nav_capabilities_pub(-1),
/* states */
_att(),
_att_sp(),
_nav_capabilities(),
@ -436,8 +425,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_loiter_hold(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
land_stayonground(false),
@ -446,12 +433,16 @@ FixedwingPositionControl::FixedwingPositionControl() :
launch_detected(false),
usePreTakeoffThrust(false),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
target_bearing(0.0f),
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
_airspeed_last_valid(0),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
_l1_control(),
_mTecs(),
_was_pos_control_mode(false)
{
@ -572,6 +563,23 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_pitch_damping(_parameters.pitch_damping);
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@ -609,17 +617,7 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_check(_control_mode_sub, &vstatus_updated);
if (vstatus_updated) {
bool was_armed = _control_mode.flag_armed;
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
}
}
@ -643,6 +641,9 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
@ -703,7 +704,17 @@ FixedwingPositionControl::vehicle_setpoint_poll()
void
FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
{
l1_control::g_control = new FixedwingPositionControl();
if (l1_control::g_control == nullptr) {
warnx("OUT OF MEM");
return;
}
/* only returns on exit */
l1_control::g_control->task_main();
delete l1_control::g_control;
l1_control::g_control = nullptr;
}
float
@ -817,7 +828,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@ -842,6 +859,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
@ -949,7 +969,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
/* Calculate altitude of last ordinary waypoint L */
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@ -1115,15 +1135,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
// (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
/* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
reset_landing_state();
@ -1139,89 +1150,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (0/* posctrl mode enabled */) {
_was_pos_control_mode = false;
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
// climb_out = true;
// }
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (0/* altctrl mode enabled */) {
_was_pos_control_mode = false;
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed);
} else {
_was_pos_control_mode = false;
@ -1242,9 +1170,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _mTecs.getPitchSetpoint();
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@ -1260,10 +1188,6 @@ void
FixedwingPositionControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
@ -1339,14 +1263,6 @@ FixedwingPositionControl::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
@ -1432,20 +1348,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
const math::Vector<3> &ground_speed,
tecs_mode mode)
{
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
limitOverride.disablePitchMinOverride();
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
}
int
@ -1479,14 +1404,7 @@ int fw_pos_control_l1_main(int argc, char *argv[])
if (l1_control::g_control != nullptr)
errx(1, "already running");
l1_control::g_control = new FixedwingPositionControl;
if (l1_control::g_control == nullptr)
errx(1, "alloc failed");
if (OK != l1_control::g_control->start()) {
delete l1_control::g_control;
l1_control::g_control = nullptr;
if (OK != FixedwingPositionControl::start()) {
err(1, "start failed");
}

11
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -59,6 +59,7 @@ mTecs::mTecs() :
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@ -75,6 +76,7 @@ mTecs::mTecs() :
_counter(0),
_debug(false)
{
warnx("starting");
}
mTecs::~mTecs()
@ -93,18 +95,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* time measurement */
updateTimeMeasurement();
/* Filter altitude */
float altitudeFiltered = _altitudeLowpass.update(altitude);
/* calculate flight path angle setpoint from altitude setpoint */
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */

1
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -115,6 +115,7 @@ protected:
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */

Some files were not shown because too many files have changed in this diff Show More