fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 12:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into attitudeekf

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-11 16:17:27 +02:00
parent 963394f0e4 5abdacc907
commit 9e9da83cb7
70 changed files with 2847 additions and 1846 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/3035_viper
+10
View File
@@ -0,0 +1,10 @@
#!nsh
#
# Viper
#
# Simon Wilks <sjwilks@gmail.com>
#
sh /etc/init.d/rc.fw_defaults
set MIXER Viper
ROMFS/px4fmu_common/init.d/rc.autostart
+5
View File
@@ -103,6 +103,11 @@ then
sh /etc/init.d/3034_fx79
fi
if param compare SYS_AUTOSTART 3035 35
then
sh /etc/init.d/3035_viper
fi
if param compare SYS_AUTOSTART 3100
then
sh /etc/init.d/3100_tbs_caipirinha
ROMFS/px4fmu_common/init.d/rc.fw_defaults
+4
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
ROMFS/px4fmu_common/init.d/rc.usb
+7 -1
View File
@@ -3,8 +3,12 @@
# USB MAVLink start
#
mavlink start -r 10000 -d /dev/ttyACM0 -x
mavlink start -r 20000 -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
ROMFS/px4fmu_common/mixers/FMU_FX79.mix
+2 -2
View File
@@ -27,12 +27,12 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 5000 8000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 8000 5000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
ROMFS/px4fmu_common/mixers/Viper.mix
Executable
+72
View File
@@ -0,0 +1,72 @@
Viper Delta-wing mixer
=================================
Designed for Viper.
TODO (sjwilks): Add mixers for flaps.
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
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 for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 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
Tools/fetch_file.py
+207
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()
Tools/fetch_log.py
-133
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()
Tools/sdlog2/sdlog2_dump.py
+2 -1
View File
@@ -149,7 +149,8 @@ class SDLog2Parser:
break
if first_data_msg:
# build CSV columns and init data map
self.__initCSV()
if not self.__debug_out:
self.__initCSV()
first_data_msg = False
self.__parseMsg(msg_descr)
bytes_read += self.__ptr
makefiles/config_aerocore_default.mk
+1
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
makefiles/config_px4fmu-v1_default.mk
+1
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
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -121,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
mavlink/include/mavlink/v1.0
+1 -1
Submodule mavlink/include/mavlink/v1.0 updated: 04b1ad5b28...4cd384001b
src/drivers/drv_tone_alarm.h
+1
View File
@@ -150,6 +150,7 @@ enum {
TONE_ARMING_FAILURE_TUNE,
TONE_PARACHUTE_RELEASE_TUNE,
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
TONE_NUMBER_OF_TUNES
};
src/drivers/meas_airspeed/meas_airspeed.cpp
+4 -4
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.2f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeed : public Airspeed
{
src/drivers/px4io/px4io_serial.cpp
+7 -1
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 ")),
src/drivers/px4io/px4io_uploader.cpp
+15 -14
View File
@@ -65,7 +65,8 @@
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
_fw_fd(-1),
bl_rev(0)
{
}
@@ -245,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];
@@ -262,19 +263,19 @@ 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;
int ret = OK;
while (count--) {
ret = recv(*p++, 5000);
ret = recv_byte_with_timeout(p++, 5000);
if (ret != OK)
break;
@@ -289,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) {
@@ -331,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;
@@ -372,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;
@@ -513,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);
@@ -600,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");
src/drivers/px4io/uploader.h
+4 -4
View File
@@ -74,19 +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 */
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);
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+2
View File
@@ -337,6 +337,7 @@ ToneAlarm::ToneAlarm() :
_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
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -350,6 +351,7 @@ ToneAlarm::ToneAlarm() :
_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
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
}
ToneAlarm::~ToneAlarm()
src/lib/ecl/module.mk
+2
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
src/lib/external_lgpl/module.mk
+2
View File
@@ -46,3 +46,5 @@
#
SRCS = tecs/tecs.cpp
MAXOPTIMIZATION = -Os
src/lib/external_lgpl/tecs/tecs.cpp
+31 -16
View File
@@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
@@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
_throttle_dem = constrain(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Ensure _last_throttle_dem is always initialized properly
// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
// Calculate integrator state upper and lower limits
// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
@@ -551,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// Calculate pitch demand
_update_pitch();
// // Write internal variables to the log_tuning structure. This
// // structure will be logged in dataflash at 10Hz
// log_tuning.hgt_dem = _hgt_dem_adj;
// log_tuning.hgt = _integ3_state;
// log_tuning.dhgt_dem = _hgt_rate_dem;
// log_tuning.dhgt = _integ2_state;
// log_tuning.spd_dem = _TAS_dem_adj;
// log_tuning.spd = _integ5_state;
// log_tuning.dspd = _vel_dot;
// log_tuning.ithr = _integ6_state;
// log_tuning.iptch = _integ7_state;
// log_tuning.thr = _throttle_dem;
// log_tuning.ptch = _pitch_dem;
// log_tuning.dspd_dem = _TAS_rate_dem;
_tecs_state.timestamp = now;
if (_underspeed) {
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_badDescent) {
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbOutDem) {
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// If no error flag applies, conclude normal
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
}
src/lib/external_lgpl/tecs/tecs.h
+55 -17
View File
@@ -28,6 +28,27 @@ class __EXPORT TECS
{
public:
TECS() :
_tecs_state {},
_update_50hz_last_usec(0),
_update_speed_last_usec(0),
_update_pitch_throttle_last_usec(0),
// TECS tuning parameters
_hgtCompFiltOmega(0.0f),
_spdCompFiltOmega(0.0f),
_maxClimbRate(2.0f),
_minSinkRate(1.0f),
_maxSinkRate(2.0f),
_timeConst(5.0f),
_timeConstThrot(8.0f),
_ptchDamp(0.0f),
_thrDamp(0.0f),
_integGain(0.0f),
_vertAccLim(0.0f),
_rollComp(0.0f),
_spdWeight(0.5f),
_heightrate_p(0.0f),
_speedrate_p(0.0f),
_throttle_dem(0.0f),
_pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
@@ -100,29 +121,42 @@ public:
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT,
ECL_TECS_MODE_CLIMBOUT
};
// struct PACKED log_TECS_Tuning {
// LOG_PACKET_HEADER;
// float hgt;
// float dhgt;
// float hgt_dem;
// float dhgt_dem;
// float spd_dem;
// float spd;
// float dspd;
// float ithr;
// float iptch;
// float thr;
// float ptch;
// float dspd_dem;
// } log_tuning;
struct tecs_state {
uint64_t timestamp;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
enum ECL_TECS_MODE mode;
};
void get_tecs_state(struct tecs_state& state) {
state = _tecs_state;
}
void set_time_const(float time_const) {
_timeConst = time_const;
}
void set_time_const_throt(float time_const_throt) {
_timeConstThrot = time_const_throt;
}
void set_min_sink_rate(float rate) {
_minSinkRate = rate;
}
@@ -188,6 +222,9 @@ public:
}
private:
struct tecs_state _tecs_state;
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@@ -204,6 +241,7 @@ private:
float _minSinkRate;
float _maxSinkRate;
float _timeConst;
float _timeConstThrot;
float _ptchDamp;
float _thrDamp;
float _integGain;
src/modules/commander/commander.cpp
+28 -24
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));
@@ -391,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -736,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);
@@ -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;
@@ -1081,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_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 {
@@ -1284,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1305,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1364,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1390,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1502,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;
@@ -2152,7 +2156,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -2215,7 +2219,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+5 -5
View File
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
src/modules/commander/state_machine_helper.cpp
+10 -7
View File
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
int prearm_ret = OK;
/* only perform the check if we have to */
if (new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@@ -669,7 +670,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));
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -57,7 +57,7 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
src/modules/controllib/blocks.cpp
+3
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());
src/modules/controllib/blocks.hpp
+1 -1
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() {};
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+4 -23
View File
@@ -630,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);
@@ -1465,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];
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+33 -4
View File
@@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED()
// apply a 5-sigma threshold
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
{
if (current_ekf_state.velHealth || staticMode) {
current_ekf_state.velHealth = true;
current_ekf_state.velFailTime = millis();
} else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
// XXX check
current_ekf_state.velHealth = true;
ResetVelocity();
ResetStoredStates();
// do not fuse bad data
fuseVelData = false;
}
else
{
@@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED()
{
current_ekf_state.posHealth = true;
current_ekf_state.posFailTime = millis();
if (current_ekf_state.posTimeout) {
ResetPosition();
// XXX cross-check the state reset
ResetStoredStates();
// do not fuse position data on this time
// step
fusePosData = false;
}
}
else
{
@@ -1070,10 +1087,18 @@ void AttPosEKF::FuseVelposNED()
// apply a 10-sigma threshold
current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
{
current_ekf_state.hgtHealth = true;
current_ekf_state.hgtFailTime = millis();
// if we just reset from a timeout, do not fuse
// the height data, but reset height and stored states
if (current_ekf_state.hgtTimeout) {
ResetHeight();
ResetStoredStates();
fuseHgtData = false;
}
}
else
{
@@ -2773,7 +2798,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 +3053,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));
src/modules/ekf_att_pos_estimator/estimator_utilities.h
+4
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;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+196 -33
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,7 +134,6 @@ 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;
@@ -144,6 +146,7 @@ private:
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
@@ -192,6 +195,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;
@@ -199,6 +203,23 @@ private:
float l1_period;
float l1_damping;
float time_const;
float time_const_throt;
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float climbout_diff;
float heightrate_p;
float speedrate_p;
float throttle_damp;
float integrator_gain;
float vertical_accel_limit;
float height_comp_filter_omega;
float speed_comp_filter_omega;
float roll_throttle_compensation;
float speed_weight;
float pitch_damping;
float airspeed_min;
float airspeed_trim;
float airspeed_max;
@@ -209,6 +230,7 @@ private:
float throttle_min;
float throttle_max;
float throttle_cruise;
float throttle_slew_max;
float throttle_land_max;
@@ -226,6 +248,23 @@ private:
param_t l1_period;
param_t l1_damping;
param_t time_const;
param_t time_const_throt;
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t climbout_diff;
param_t heightrate_p;
param_t speedrate_p;
param_t throttle_damp;
param_t integrator_gain;
param_t vertical_accel_limit;
param_t height_comp_filter_omega;
param_t speed_comp_filter_omega;
param_t roll_throttle_compensation;
param_t speed_weight;
param_t pitch_damping;
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
@@ -236,6 +275,7 @@ private:
param_t throttle_min;
param_t throttle_max;
param_t throttle_cruise;
param_t throttle_slew_max;
param_t throttle_land_max;
@@ -361,7 +401,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false),
_task_running(false),
_control_task(-1),
/* subscriptions */
_global_pos_sub(-1),
@@ -376,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* publications */
_attitude_sp_pub(-1),
_tecs_status_pub(-1),
_nav_capabilities_pub(-1),
/* states */
@@ -428,6 +468,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.roll_limit = param_find("FW_R_LIM");
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
_parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
@@ -438,6 +479,23 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
_parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA");
_parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA");
_parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
/* fetch initial parameter values */
parameters_update();
}
@@ -485,9 +543,28 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
@@ -500,6 +577,25 @@ 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_time_const_throt(_parameters.time_const_throt);
_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_throttle_slewrate(_parameters.throttle_slew_max);
_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 ||
@@ -561,6 +657,9 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
@@ -621,7 +720,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
@@ -733,6 +842,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* 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;
@@ -758,6 +875,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);
@@ -991,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@@ -1066,9 +1186,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;
@@ -1084,10 +1204,6 @@ void
FixedwingPositionControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
@@ -1248,20 +1364,74 @@ 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);
struct TECS::tecs_state s;
_tecs.get_tecs_state(s);
struct tecs_status_s t;
t.timestamp = s.timestamp;
switch (s.mode) {
case TECS::ECL_TECS_MODE_NORMAL:
t.mode = TECS_MODE_NORMAL;
break;
case TECS::ECL_TECS_MODE_UNDERSPEED:
t.mode = TECS_MODE_UNDERSPEED;
break;
case TECS::ECL_TECS_MODE_BAD_DESCENT:
t.mode = TECS_MODE_BAD_DESCENT;
break;
case TECS::ECL_TECS_MODE_CLIMBOUT:
t.mode = TECS_MODE_CLIMBOUT;
break;
}
t.altitudeSp = s.hgt_dem;
t.altitude_filtered = s.hgt;
t.airspeedSp = s.spd_dem;
t.airspeed_filtered = s.spd;
t.flightPathAngleSp = s.dhgt_dem;
t.flightPathAngle = s.dhgt;
t.flightPathAngleFiltered = s.dhgt;
t.airspeedDerivativeSp = s.dspd_dem;
t.airspeedDerivative = s.dspd;
t.totalEnergyRateSp = s.thr;
t.totalEnergyRate = s.ithr;
t.energyDistributionRateSp = s.ptch;
t.energyDistributionRate = s.iptch;
if (_tecs_status_pub > 0) {
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
} else {
_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
}
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
}
int
@@ -1295,14 +1465,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");
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+210
View File
@@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
*/
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
/**
* Throttle max slew rate
*
* Maximum slew rate for the commanded throttle
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
/**
* Negative pitch limit
*
@@ -154,6 +165,205 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
* Climbout Altitude difference
*
* If the altitude error exceeds this parameter, the system will climb out
* with maximum throttle and minimum airspeed until it is closer than this
* distance to the desired altitude. Mostly used for takeoff waypoints / modes.
* Set to zero to disable climbout mode (not recommended).
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
/**
* Maximum climb rate
*
* This is the best climb rate that the aircraft can achieve with
* the throttle set to THR_MAX and the airspeed set to the
* default value. For electric aircraft make sure this number can be
* achieved towards the end of flight when the battery voltage has reduced.
* The setting of this parameter can be checked by commanding a positive
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
* required to climb is close to THR_MAX and the aircraft is maintaining
* airspeed, then this parameter is set correctly. If the airspeed starts
* to reduce, then the parameter is set to high, and if the throttle
* demand required to climb and maintain speed is noticeably less than
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* FW_THR_MAX reduced.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
/**
* Minimum descent rate
*
* This is the sink rate of the aircraft with the throttle
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
/**
* Maximum descent rate
*
* This sets the maximum descent rate that the controller will use.
* If this value is too large, the aircraft can over-speed on descent.
* This should be set to a value that can be achieved without
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
/**
* TECS time constant
*
* This is the time constant of the TECS control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* TECS Throttle time constant
*
* This is the time constant of the TECS throttle control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
/**
* Integrator gain
*
* This is the integrator gain on the control loop.
* Increasing this gain increases the speed at which speed
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
/**
* Maximum vertical acceleration
*
* This is the maximum vertical acceleration (in metres/second square)
* either up or down that the controller will use to correct speed
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
/**
* Complementary filter "omega" parameter for height
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse vertical acceleration and barometric height to obtain
* an estimate of height rate and height. Increasing this frequency weights
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
/**
* Complementary filter "omega" parameter for speed
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* improved airspeed estimate. Increasing this frequency weights the solution
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
/**
* Roll -> Throttle feedforward
*
* Increasing this gain turn increases the amount of throttle that will
* be used to compensate for the additional drag created by turning.
* Ideally this should be set to approximately 10 x the extra sink rate
* in m/s created by a 45 degree bank turn. Increase this gain if
* the aircraft initially loses energy in turns and reduce if the
* aircraft initially gains energy in turns. Efficient high aspect-ratio
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
/**
* Speed <--> Altitude priority
*
* This parameter adjusts the amount of weighting that the pitch control
* applies to speed vs height errors. Setting it to 0.0 will cause the
* pitch control to control height and ignore speed errors. This will
* normally improve height accuracy but give larger airspeed errors.
* Setting it to 2.0 will cause the pitch control loop to control speed
* and ignore height errors. This will normally reduce airspeed errors,
* but give larger height errors. The default value of 1.0 allows the pitch
* control to simultaneously control height and speed.
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
/**
* Pitch damping factor
*
* This is the damping gain for the pitch demand loop. Increase to add
* damping to correct for oscillations in height. The default value of 0.0
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate P factor
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Speed rate P factor
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
/**
* Landing slope angle
*
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+2 -4
View File
@@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
_status.altitude_filtered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
@@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
_status.airspeed_filtered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+10 -10
View File
@@ -55,7 +55,7 @@
* @max 1
* @group mTECS
*/
PARAM_DEFINE_INT32(MT_ENABLED, 1);
PARAM_DEFINE_INT32(MT_ENABLED, 0);
/**
* Total Energy Rate Control Feedforward
@@ -241,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f);
/**
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f);
/**
* P gain for the airspeed control
@@ -268,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f);
/**
* Minimal acceleration (air)
@@ -286,13 +293,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
*/
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
/**
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
/**
* Minimal throttle during takeoff
*
src/modules/mavlink/mavlink_bridge_header.h
+6 -1
View File
@@ -44,7 +44,12 @@
__BEGIN_DECLS
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
/*
* We are NOT using convenience functions,
* but instead send messages with a custom function.
* So we do NOT do this:
* #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
*/
/* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
src/modules/mavlink/mavlink_commands.cpp
-73
View File
@@ -1,73 +0,0 @@
/****************************************************************************
*
* 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 mavlink_commands.cpp
* Mavlink commands stream implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "mavlink_commands.h"
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) :
_cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
_cmd{},
_channel(channel),
_cmd_time(0)
{
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
struct vehicle_command_s cmd;
if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
cmd.target_system,
cmd.target_component,
cmd.command,
cmd.confirmation,
cmd.param1,
cmd.param2,
cmd.param3,
cmd.param4,
cmd.param5,
cmd.param6,
cmd.param7);
}
}
}
src/modules/mavlink/mavlink_ftp.cpp
+41 -7
View File
@@ -35,6 +35,7 @@
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <sys/stat.h>
#include "mavlink_ftp.h"
@@ -190,6 +191,8 @@ void
MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
hdr->magic = kProtocolMagic;
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
@@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req)
break;
}
// name too big to fit?
if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
break;
}
uint32_t fileSize = 0;
char buf[256];
// store the type marker
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
struct stat st;
if (stat(buf, &st) == 0) {
fileSize = st.st_size;
}
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
@@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req)
hdr->data[offset++] = kDirentUnknown;
break;
}
if (entry.d_type == DTYPE_FILE) {
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
} else {
strncpy(buf, entry.d_name, sizeof(buf));
buf[sizeof(buf)-1] = 0;
}
size_t nameLen = strlen(buf);
// name too big to fit?
if ((nameLen + offset + 2) > kMaxDataLength) {
break;
}
// copy the name, which we know will fit
strcpy((char *)&hdr->data[offset], entry.d_name);
strcpy((char *)&hdr->data[offset], buf);
//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
offset += strlen(entry.d_name) + 1;
offset += nameLen + 1;
}
closedir(dp);
@@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create)
return kErrNoSession;
}
uint32_t fileSize = 0;
if (!create) {
struct stat st;
if (stat(req->dataAsCString(), &st) != 0) {
return kErrNotFile;
}
fileSize = st.st_size;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
@@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create)
_session_fds[session_index] = fd;
hdr->session = session_index;
hdr->size = 0;
if (create) {
hdr->size = 0;
} else {
hdr->size = sizeof(uint32_t);
*((uint32_t*)hdr->data) = fileSize;
}
return kErrNone;
}
src/modules/mavlink/mavlink_ftp.h
+2 -1
View File
@@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include "mavlink_messages.h"
#include "mavlink_main.h"
class MavlinkFTP
{
@@ -145,7 +146,7 @@ private:
mavlink_message_t msg;
msg.checksum = 0;
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence(), rawData());
_mavlink->get_channel(), &msg, sequence()+1, rawData());
_mavlink->lockMessageBufferMutex();
bool success = _mavlink->message_buffer_write(&msg, len);
src/modules/mavlink/mavlink_main.cpp
+311 -387
View File
@@ -78,7 +78,6 @@
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
#ifndef MAVLINK_CRC_EXTRA
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
@@ -91,14 +90,19 @@
static const int ERROR = -1;
#define DEFAULT_DEVICE_NAME "/dev/ttyS1"
#define MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAX_DATA_RATE 20000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate
#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN
static Mavlink *_mavlink_instances = nullptr;
/* TODO: if this is a class member it crashes */
static struct file_operations fops;
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
/**
* mavlink app start / stop handling function
*
@@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
extern mavlink_system_t mavlink_system;
static uint64_t last_write_success_times[6] = {0};
static uint64_t last_write_try_times[6] = {0};
/*
* Internal function to send the bytes through the right serial port
*/
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
Mavlink *instance;
switch (channel) {
case MAVLINK_COMM_0:
instance = Mavlink::get_instance(0);
break;
case MAVLINK_COMM_1:
instance = Mavlink::get_instance(1);
break;
case MAVLINK_COMM_2:
instance = Mavlink::get_instance(2);
break;
case MAVLINK_COMM_3:
instance = Mavlink::get_instance(3);
break;
#ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
instance = Mavlink::get_instance(4);
break;
#endif
#ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
instance = Mavlink::get_instance(5);
break;
#endif
#ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6:
instance = Mavlink::get_instance(6);
break;
#endif
default:
return;
}
int uart = instance->get_uart_fd();
ssize_t desired = (sizeof(uint8_t) * length);
/*
* Check if the OS buffer is full and disable HW
* flow control if it continues to be full
*/
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
if (last_write_try_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
last_write_success_times[(unsigned)channel] !=
last_write_try_times[(unsigned)channel]) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
}
}
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
last_write_try_times[(unsigned)channel] = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
if (buf_free < desired) {
/* we don't want to send anything just in half, so return */
instance->count_txerr();
instance->count_txerrbytes(desired);
return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
instance->count_txerr();
instance->count_txerrbytes(desired);
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
instance->count_txbytes(desired);
}
}
}
static void usage(void);
Mavlink::Mavlink() :
@@ -233,8 +130,7 @@ Mavlink::Mavlink() :
_subscriptions(nullptr),
_streams(nullptr),
_mission_manager(nullptr),
_mission_pub(-1),
_mission_result_sub(-1),
_parameters_manager(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
@@ -246,12 +142,16 @@ Mavlink::Mavlink() :
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(10000),
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
_bytes_tx(0),
_bytes_txerr(0),
_bytes_rx(0),
@@ -262,6 +162,7 @@ Mavlink::Mavlink() :
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
_send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
@@ -483,7 +384,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
inst->pass_message(msg);
/* if not in normal mode, we are an onboard link
* onboard links should only pass on messages from the same system ID */
if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
inst->pass_message(msg);
}
}
}
}
@@ -531,10 +437,26 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
const char *txt = (const char *)arg;
// printf("logmsg: %s\n", txt);
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
msg.severity = (unsigned char)cmd;
switch (cmd) {
case MAVLINK_IOC_SEND_TEXT_INFO:
msg.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
msg.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
msg.severity = MAV_SEVERITY_EMERGENCY;
break;
default:
msg.severity = MAV_SEVERITY_INFO;
break;
}
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
@@ -559,22 +481,39 @@ void Mavlink::mavlink_update_system(void)
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_initialized = true;
}
/* update system and component id */
int32_t system_id;
param_get(_param_system_id, &system_id);
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
int32_t component_id;
param_get(_param_component_id, &component_id);
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
/* only allow system ID and component ID updates
* after reboot - not during operation */
if (!_param_initialized) {
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
_param_initialized = true;
}
/* warn users that they need to reboot to take this
* into effect
*/
if (system_id != mavlink_system.sysid) {
send_statustext_critical("Save params and reboot to change SYSID");
}
if (component_id != mavlink_system.compid) {
send_statustext_critical("Save params and reboot to change COMPID");
}
int32_t system_type;
@@ -711,6 +650,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
if (enable_flow_control(true)) {
warnx("hardware flow control not supported");
}
} else {
_flow_control_enabled = false;
}
return _uart_fd;
@@ -752,8 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled)
/* enable HIL */
if (hil_enabled && !_hil_enabled) {
_hil_enabled = true;
float rate_mult = _datarate / 1000.0f;
configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
configure_stream("HIL_CONTROLS", 200.0f);
}
/* disable HIL */
@@ -768,13 +709,145 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
void
Mavlink::send_message(const mavlink_message_t *msg)
unsigned
Mavlink::get_free_tx_buf()
{
/*
* Check if the OS buffer is full and disable HW
* flow control if it continues to be full
*/
int buf_free = 0;
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) {
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
if (_last_write_try_time != 0 &&
hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
_last_write_success_time != _last_write_try_time) {
warnx("DISABLING HARDWARE FLOW CONTROL");
enable_flow_control(false);
}
}
return buf_free;
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* use mavlink's internal counter for the TX seq */
buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
buf[3] = mavlink_system.sysid;
buf[4] = mavlink_system.compid;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
void
Mavlink::resend_message(mavlink_message_t *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header and payload */
memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
/* checksum */
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
void
@@ -784,7 +857,7 @@ Mavlink::handle_message(const mavlink_message_t *msg)
_mission_manager->handle_message(msg);
/* handle packet with parameter component */
mavlink_pm_message_handler(_channel, msg);
_parameters_manager->handle_message(msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
@@ -792,224 +865,32 @@ Mavlink::handle_message(const mavlink_message_t *msg)
}
}
int
Mavlink::mavlink_pm_queued_send()
{
if (_mavlink_param_queue_index < param_count()) {
mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
_mavlink_param_queue_index++;
return 0;
} else {
return 1;
}
}
void Mavlink::mavlink_pm_start_queued_send()
{
_mavlink_param_queue_index = 0;
}
int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
{
return mavlink_pm_send_param(param_for_index(index));
}
int Mavlink::mavlink_pm_send_param_for_name(const char *name)
{
return mavlink_pm_send_param(param_find(name));
}
int Mavlink::mavlink_pm_send_param(param_t param)
{
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
/* copy parameter name */
strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
uint8_t mavlink_type;
if (type == PARAM_TYPE_INT32) {
mavlink_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
mavlink_type = MAVLINK_TYPE_FLOAT;
} else {
mavlink_type = MAVLINK_TYPE_FLOAT;
}
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
int ret;
if ((ret = param_get(param, &val_buf)) != OK) {
return ret;
}
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
_channel,
&tx_msg,
name_buf,
val_buf,
mavlink_type,
param_count(),
param_get_index(param));
send_message(&tx_msg);
return OK;
}
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_param_request_list_t req;
mavlink_msg_param_request_list_decode(msg, &req);
if (req.target_system == mavlink_system.sysid &&
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
send_statustext_info("[pm] sending list");
}
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
/* Handle parameter setting */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t mavlink_param_set;
mavlink_msg_param_set_decode(msg, &mavlink_param_set);
if (mavlink_param_set.target_system == mavlink_system.sysid
&& ((mavlink_param_set.target_component == mavlink_system.compid)
|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */
param_t param = param_find(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown: %s", name);
send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(mavlink_param_set.param_value));
mavlink_pm_send_param(param);
}
}
}
} break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
if (mavlink_param_request_read.target_system == mavlink_system.sysid
&& ((mavlink_param_request_read.target_component == mavlink_system.compid)
|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
/* when no index is given, loop through string ids and compare them */
if (mavlink_param_request_read.param_index == -1) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
mavlink_pm_send_param_for_name(name);
} else {
/* when index is >= 0, send this parameter again */
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
}
}
} break;
}
}
int
void
Mavlink::send_statustext_info(const char *string)
{
return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
send_statustext(MAV_SEVERITY_INFO, string);
}
int
void
Mavlink::send_statustext_critical(const char *string)
{
return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
send_statustext(MAV_SEVERITY_CRITICAL, string);
}
int
void
Mavlink::send_statustext_emergency(const char *string)
{
return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
send_statustext(MAV_SEVERITY_EMERGENCY, string);
}
int
Mavlink::send_statustext(unsigned severity, const char *string)
void
Mavlink::send_statustext(unsigned char severity, const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
struct mavlink_logmessage logmsg;
strncpy(logmsg.text, string, sizeof(logmsg.text));
logmsg.severity = severity;
int i = 0;
while (i < len - 1) {
statustext.text[i] = string[i];
if (string[i] == '\0') {
break;
}
i++;
}
if (i > 1) {
/* Enforce null termination */
statustext.text[i] = '\0';
/* Map severity */
switch (severity) {
case MAVLINK_IOC_SEND_TEXT_INFO:
statustext.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
statustext.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
statustext.severity = MAV_SEVERITY_EMERGENCY;
break;
}
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
return OK;
} else {
return ERROR;
}
mavlink_logbuffer_write(&_logbuffer, &logmsg);
}
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
@@ -1032,11 +913,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
return sub_new;
}
unsigned int
Mavlink::interval_from_rate(float rate)
{
return (rate > 0.0f) ? (1000000.0f / rate) : 0;
}
int
Mavlink::configure_stream(const char *stream_name, const float rate)
{
/* calculate interval in us, 0 means disabled stream */
unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
unsigned int interval = interval_from_rate(rate);
/* search if stream exists */
MavlinkStream *stream;
@@ -1067,10 +954,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
stream = streams_list[i]->new_instance();
stream->set_channel(get_channel());
stream = streams_list[i]->new_instance(this);
stream->set_interval(interval);
stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
@@ -1083,6 +968,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
return ERROR;
}
void
Mavlink::adjust_stream_rates(const float multiplier)
{
/* do not allow to push us to zero */
if (multiplier < 0.01f) {
return;
}
/* search if stream exists */
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
/* set new interval */
unsigned interval = stream->get_interval();
interval /= multiplier;
/* allow max ~600 Hz */
if (interval < 1600) {
interval = 1600;
}
/* set new interval */
stream->set_interval(interval * multiplier);
}
}
void
Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
{
@@ -1243,7 +1153,27 @@ Mavlink::pass_message(const mavlink_message_t *msg)
float
Mavlink::get_rate_mult()
{
return _datarate / 1000.0f;
return _rate_mult;
}
void
Mavlink::update_rate_mult()
{
float const_rate = 0.0f;
float rate = 0.0f;
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
if (stream->const_rate()) {
const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
} else {
rate += stream->get_size() * 1000000.0f / stream->get_interval();
}
}
/* don't scale up rates, only scale down if needed */
_rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
}
int
@@ -1381,6 +1311,9 @@ Mavlink::task_main(int argc, char *argv[])
return ERROR;
}
/* initialize send mutex */
pthread_mutex_init(&_send_mutex, NULL);
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
@@ -1390,7 +1323,7 @@ Mavlink::task_main(int argc, char *argv[])
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -1410,12 +1343,6 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
/* create mission manager */
_mission_manager = new MavlinkMissionManager(this);
_mission_manager->set_verbose(_verbose);
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
@@ -1426,34 +1353,50 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = get_rate_mult();
/* add default streams depending on mode */
/* HEARTBEAT is constant rate stream, rate never adjusted */
configure_stream("HEARTBEAT", 1.0f);
/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
configure_stream("STATUSTEXT", 20.0f);
/* COMMAND_LONG stream: use high rate to avoid commands skipping */
configure_stream("COMMAND_LONG", 100.0f);
/* PARAM_VALUE stream */
_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
_parameters_manager->set_interval(interval_from_rate(30.0f));
LL_APPEND(_streams, _parameters_manager);
/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
* remote requests rate. Rate specified here controls how much bandwidth we will reserve for
* mission messages. */
_mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this);
_mission_manager->set_interval(interval_from_rate(10.0f));
_mission_manager->set_verbose(_verbose);
LL_APPEND(_streams, _mission_manager);
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
configure_stream("ATTITUDE", 10.0f * rate_mult);
configure_stream("VFR_HUD", 8.0f * rate_mult);
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
configure_stream("HIGHRES_IMU", 1.0f);
configure_stream("ATTITUDE", 10.0f);
configure_stream("VFR_HUD", 8.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS_RAW", 1.0f);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
configure_stream("ATTITUDE", 15.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
@@ -1461,13 +1404,8 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
/* don't send parameters on startup without request */
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
@@ -1480,6 +1418,8 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
update_rate_mult();
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
@@ -1490,9 +1430,6 @@ Mavlink::task_main(int argc, char *argv[])
set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
/* update commands stream */
commands_stream.update(t);
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
@@ -1518,20 +1455,6 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
_mission_manager->eventloop();
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
if (lb_ret == OK) {
send_statustext(msg.severity, msg.text);
}
}
}
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
@@ -1576,7 +1499,7 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
resend_message(&msg);
}
}
@@ -1591,14 +1514,13 @@ Mavlink::task_main(int argc, char *argv[])
_bytes_txerr = 0;
_bytes_rx = 0;
}
_bytes_timestamp = t;
}
perf_end(_loop_perf);
}
delete _mission_manager;
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;
@@ -1756,10 +1678,12 @@ Mavlink::display_status()
} else {
printf("\tno telem status.\n");
}
printf("\trates:\n");
printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
printf("\trx: %.3f kB/s\n", (double)_rate_rx);
printf("\trate mult: %.3f\n", (double)_rate_mult);
}
int
src/modules/mavlink/mavlink_main.h
+47 -52
View File
@@ -58,7 +58,7 @@
#include "mavlink_stream.h"
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
class Mavlink
{
@@ -160,7 +160,12 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
void send_message(const mavlink_message_t *msg);
void send_message(const uint8_t msgid, const void *msg);
/**
* Resend message as is, don't change sequence number and CRC.
*/
void resend_message(mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
@@ -188,29 +193,33 @@ public:
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_info(const char *string);
void send_statustext_info(const char *string);
/**
* Send a status text with loglevel CRITICAL
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_critical(const char *string);
void send_statustext_critical(const char *string);
/**
* Send a status text with loglevel EMERGENCY
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_emergency(const char *string);
void send_statustext_emergency(const char *string);
/**
* Send a status text with loglevel
* Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
* only on this mavlink connection. Useful for reporting communication specific, not system-wide info
* only to client interested in it. Message will be not sent immediately but queued in buffer as
* for mavlink_log_xxx().
*
* @param string the message to send (will be capped by mavlink max string length)
* @param severity the log level, one of
* @param severity the log level
*/
int send_statustext(unsigned severity, const char *string);
void send_statustext(unsigned char severity, const char *string);
MavlinkStream * get_streams() const { return _streams; }
float get_rate_mult();
@@ -252,6 +261,8 @@ public:
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
protected:
Mavlink *next;
@@ -274,9 +285,8 @@ private:
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
orb_advert_t _mission_pub;
int _mission_result_sub;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
@@ -292,7 +302,9 @@ private:
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate;
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
/**
* If the queue index is not at 0, the queue sending
@@ -307,6 +319,8 @@ private:
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
uint64_t _last_write_success_time;
uint64_t _last_write_try_time;
unsigned _bytes_tx;
unsigned _bytes_txerr;
@@ -328,6 +342,7 @@ private:
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
pthread_mutex_t _send_mutex;
bool _param_initialized;
param_t _param_system_id;
@@ -338,53 +353,28 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
/**
* Send one parameter.
*
* @param param The parameter id to send.
* @return zero on success, nonzero on failure.
*/
int mavlink_pm_send_param(param_t param);
/**
* Send one parameter identified by index.
*
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
*
* This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_update_system();
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);
/**
* Adjust the stream rates based on the current rate
*
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
*/
void adjust_stream_rates(const float multiplier);
int message_buffer_init(int size);
void message_buffer_destroy();
@@ -399,6 +389,11 @@ private:
void pass_message(const mavlink_message_t *msg);
/**
* Update rate mult so total bitrate will be equal to _datarate.
*/
void update_rate_mult();
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
src/modules/mavlink/mavlink_messages.cpp
+817 -645
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_messages.h
+2 -2
View File
@@ -46,10 +46,10 @@
class StreamListItem {
public:
MavlinkStream* (*new_instance)();
MavlinkStream* (*new_instance)(Mavlink *mavlink);
const char* (*get_name)();
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
new_instance(inst),
get_name(name) {};
src/modules/mavlink/mavlink_mission.cpp
+27 -24
View File
@@ -59,8 +59,7 @@
static const int ERROR = -1;
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_mavlink(mavlink),
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
_time_last_recv(0),
_time_last_sent(0),
@@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_offboard_mission_sub(-1),
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
_slow_rate_limiter(_interval / 10.0f),
_verbose(false),
_channel(mavlink->get_channel()),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
close(_mission_result_sub);
}
unsigned
MavlinkMissionManager::get_size()
{
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
void
MavlinkMissionManager::init_offboard_mission()
{
@@ -157,15 +169,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
void
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
@@ -175,13 +185,11 @@ void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
if (seq < _count) {
mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
} else if (seq == 0 && _count == 0) {
/* don't broadcast if no WPs */
@@ -199,15 +207,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
{
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = _count;
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
@@ -226,13 +232,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
mavlink_mission_item_t wp;
format_mavlink_mission_item(&mission_item, &wp);
mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
@@ -251,13 +256,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
if (seq < _max_count) {
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
@@ -272,22 +276,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
void
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
}
void
MavlinkMissionManager::eventloop()
MavlinkMissionManager::send(const hrt_abstime now)
{
hrt_abstime now = hrt_absolute_time();
/* update interval for slow rate limiter */
_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
bool updated = false;
orb_check(_mission_result_sub, &updated);
src/modules/mavlink/mavlink_mission.h
+69 -52
View File
@@ -42,11 +42,12 @@
#pragma once
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include <uORB/uORB.h>
// FIXME XXX - TO BE MOVED TO XML
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include "mavlink_stream.h"
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
@@ -66,20 +67,75 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class Mavlink;
class MavlinkMissionManager {
class MavlinkMissionManager : public MavlinkStream {
public:
MavlinkMissionManager(Mavlink *parent);
~MavlinkMissionManager();
const char *get_name() const
{
return MavlinkMissionManager::get_name_static();
}
static const char *get_name_static()
{
return "MISSION_ITEM";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_MISSION_ITEM;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkMissionManager(mavlink);
}
unsigned get_size();
void handle_message(const mavlink_message_t *msg);
void set_verbose(bool v) { _verbose = v; }
private:
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
uint8_t _comp_id;
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
void init_offboard_mission();
int update_active_mission(int dataman_id, unsigned count, int seq);
void send_message(mavlink_message_t *msg);
/**
* @brief Sends an waypoint ack message
*/
@@ -111,10 +167,6 @@ public:
*/
void send_mission_item_reached(uint16_t seq);
void eventloop();
void handle_message(const mavlink_message_t *msg);
void handle_mission_ack(const mavlink_message_t *msg);
void handle_mission_set_current(const mavlink_message_t *msg);
@@ -139,43 +191,8 @@ public:
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void set_verbose(bool v) { _verbose = v; }
protected:
explicit MavlinkMissionManager(Mavlink *mavlink);
private:
Mavlink* _mavlink;
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
mavlink_channel_t _channel;
uint8_t _comp_id;
/* do not allow copying this class */
MavlinkMissionManager(const MavlinkMissionManager&);
MavlinkMissionManager operator=(const MavlinkMissionManager&);
void send(const hrt_abstime t);
};
src/modules/mavlink/mavlink_parameters.cpp
+197
View File
@@ -0,0 +1,197 @@
/****************************************************************************
*
* 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 mavlink_parameters.cpp
* Mavlink parameters manager implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdio.h>
#include "mavlink_parameters.h"
#include "mavlink_main.h"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1)
{
}
unsigned
MavlinkParametersManager::get_size()
{
if (_send_all_index >= 0) {
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
void
MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* request all parameters */
mavlink_param_request_list_t req_list;
mavlink_msg_param_request_list_decode(msg, &req_list);
if (req_list.target_system == mavlink_system.sysid &&
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
_send_all_index = 0;
_mavlink->send_statustext_info("[pm] sending list");
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */
param_t param = param_find(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(set.param_value));
send_param(param);
}
}
}
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
/* request one parameter */
mavlink_param_request_read_t req_read;
mavlink_msg_param_request_read_decode(msg, &req_read);
if (req_read.target_system == mavlink_system.sysid &&
(req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
/* when no index is given, loop through string ids and compare them */
if (req_read.param_index < 0) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find(name));
} else {
/* when index is >= 0, send this parameter again */
send_param(param_for_index(req_read.param_index));
}
}
break;
}
default:
break;
}
}
void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested */
if (_send_all_index >= 0) {
send_param(param_for_index(_send_all_index));
_send_all_index++;
if (_send_all_index >= (int) param_count()) {
_send_all_index = -1;
}
}
}
void
MavlinkParametersManager::send_param(param_t param)
{
if (param == PARAM_INVALID) {
return;
}
mavlink_param_value_t msg;
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return;
}
msg.param_count = param_count();
msg.param_index = param_get_index(param);
/* copy parameter name */
strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* query parameter type */
param_type_t type = param_type(param);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
if (type == PARAM_TYPE_INT32) {
msg.param_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
msg.param_type = MAVLINK_TYPE_FLOAT;
} else {
msg.param_type = MAVLINK_TYPE_FLOAT;
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
}
src/modules/mavlink/mavlink_commands.h → src/modules/mavlink/mavlink_parameters.h
+72 -22
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,34 +32,84 @@
****************************************************************************/
/**
* @file mavlink_commands.h
* Mavlink commands stream definition.
* @file mavlink_parameters.h
* Mavlink parameters manager definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_COMMANDS_H_
#define MAVLINK_COMMANDS_H_
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <systemlib/param/param.h>
class Mavlink;
class MavlinkCommansStream;
#include "mavlink_bridge_header.h"
#include "mavlink_stream.h"
#include "mavlink_main.h"
class MavlinkCommandsStream
class MavlinkParametersManager : public MavlinkStream
{
private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
void update(const hrt_abstime t);
};
const char *get_name() const
{
return MavlinkParametersManager::get_name_static();
}
#endif /* MAVLINK_COMMANDS_H_ */
static const char *get_name_static()
{
return "PARAM_VALUE";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_PARAM_VALUE;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkParametersManager(mavlink);
}
unsigned get_size();
void handle_message(const mavlink_message_t *msg);
/**
* Send one parameter identified by index.
*
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
void start_send_one(int index);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int start_send_for_name(const char *name);
/**
* Start sending the parameter queue.
*
* This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void start_send_all();
private:
int _send_all_index;
/* do not allow top copying this class */
MavlinkParametersManager(MavlinkParametersManager &);
MavlinkParametersManager& operator = (const MavlinkParametersManager &);
protected:
explicit MavlinkParametersManager(Mavlink *mavlink);
void send(const hrt_abstime t);
void send_param(param_t param);
};
src/modules/mavlink/mavlink_rate_limiter.cpp
+1 -1
View File
@@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
_last_sent = (t / _interval) * _interval;
_last_sent = t;
return true;
}
src/modules/mavlink/mavlink_receiver.cpp
+2 -35
View File
@@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_vicon_position_estimate(msg);
break;
case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
break;
case MAVLINK_MSG_ID_RADIO_STATUS:
handle_message_radio_status(msg);
break;
@@ -241,7 +237,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP ID");
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
@@ -361,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
{
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
/* Only accept system IDs from 1 to 4 */
if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
struct offboard_control_setpoint_s offboard_control_sp;
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
/* Convert values * 1000 back */
offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
}
}
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_stream.cpp
+14 -13
View File
@@ -43,9 +43,9 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream() :
MavlinkStream::MavlinkStream(Mavlink *mavlink) :
next(nullptr),
_channel(MAVLINK_COMM_0),
_mavlink(mavlink),
_interval(1000000),
_last_sent(0)
{
@@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval)
_interval = interval;
}
/**
* Set mavlink channel
*/
void
MavlinkStream::set_channel(mavlink_channel_t channel)
{
_channel = channel;
}
/**
* Update subscriptions and send message if necessary
*/
@@ -80,11 +71,21 @@ int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
unsigned int interval = _interval;
if (dt > 0 && dt >= _interval) {
if (!const_rate()) {
interval /= _mavlink->get_rate_mult();
}
if (dt > 0 && dt >= interval) {
/* interval expired, send message */
send(t);
_last_sent = (t / _interval) * _interval;
if (const_rate()) {
_last_sent = (t / _interval) * _interval;
} else {
_last_sent = t;
}
return 0;
}
src/modules/mavlink/mavlink_stream.h
+26 -7
View File
@@ -46,31 +46,50 @@
class Mavlink;
class MavlinkStream;
#include "mavlink_main.h"
class MavlinkStream
{
public:
MavlinkStream *next;
MavlinkStream();
MavlinkStream(Mavlink *mavlink);
virtual ~MavlinkStream();
/**
* Get the interval
*
* @param interval the inveral in microseconds (us) between messages
*/
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
/**
* Get the interval
*
* @return the inveral in microseconds (us) between messages
*/
unsigned get_interval() { return _interval; }
/**
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
static MavlinkStream *new_instance();
static MavlinkStream *new_instance(const Mavlink *mavlink);
static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
/**
* @return true if steam rate shouldn't be adjusted
*/
virtual bool const_rate() { return false; }
/**
* Get maximal total messages size on update
*/
virtual unsigned get_size() = 0;
protected:
mavlink_channel_t _channel;
Mavlink * _mavlink;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
src/modules/mavlink/module.mk
+1 -1
View File
@@ -40,11 +40,11 @@ SRCS += mavlink_main.cpp \
mavlink.c \
mavlink_receiver.cpp \
mavlink_mission.cpp \
mavlink_parameters.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp \
mavlink_commands.cpp \
mavlink_ftp.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/mc_pos_control/mc_pos_control_main.cpp
+145 -138
View File
@@ -105,6 +105,7 @@ public:
int start();
private:
const float alt_ctl_dz = 0.2f;
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
@@ -184,6 +185,8 @@ private:
math::Vector<3> _vel;
math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Vector<3> _sp_move_rate;
/**
* Update our local parameter cache.
@@ -216,6 +219,16 @@ private:
*/
void reset_alt_sp();
/**
* Set position setpoint using manual control
*/
void control_manual(float dt);
/**
* Set position setpoint using offboard control
*/
void control_offboard(float dt);
/**
* Select between barometric and global (AMSL) altitudes
*/
@@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel.zero();
_vel_sp.zero();
_vel_prev.zero();
_vel_ff.zero();
_sp_move_rate.zero();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
@@ -392,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@@ -497,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
@@ -508,11 +528,125 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2);
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}
void
MulticopterPositionControl::control_manual(float dt)
{
_sp_move_rate.zero();
if (_control_mode.flag_control_altitude_enabled) {
/* move altitude setpoint with throttle stick */
_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* move position setpoint with roll/pitch stick */
_sp_move_rate(0) = _manual.x;
_sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = _sp_move_rate.length();
if (sp_move_norm > 1.0f) {
_sp_move_rate /= sp_move_norm;
}
/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
}
void
MulticopterPositionControl::control_offboard(float dt)
{
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
/* control position */
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* set altitude setpoint move rate */
_sp_move_rate(2) = _pos_sp_triplet.current.vz;
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
} else {
reset_pos_sp();
reset_alt_sp();
}
}
void
MulticopterPositionControl::task_main()
{
@@ -551,13 +685,6 @@ MulticopterPositionControl::task_main()
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
float yaw_sp_move_rate;
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
@@ -616,138 +743,17 @@ MulticopterPositionControl::task_main()
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
sp_move_rate.zero();
_vel_ff.zero();
_sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _manual.x;
sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
control_manual(dt);
} else if (_control_mode.flag_control_offboard_enabled) {
/* Offboard control */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _pos_sp_triplet.current.vx;
sp_move_rate(1) = _pos_sp_triplet.current.vy;
yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
}
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else {
reset_pos_sp();
reset_alt_sp();
}
/* offboard control */
control_offboard(dt);
} else {
/* AUTO */
@@ -782,6 +788,7 @@ MulticopterPositionControl::task_main()
}
/* fill local position setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
@@ -821,7 +828,7 @@ MulticopterPositionControl::task_main()
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
@@ -901,7 +908,7 @@ MulticopterPositionControl::task_main()
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
src/modules/navigator/mission.cpp
+25 -15
View File
@@ -273,6 +273,10 @@ Mission::check_dist_1wp()
if (dist_to_1wp < _param_dist_1wp.get()) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
}
return true;
} else {
@@ -314,30 +318,36 @@ Mission::set_mission_items()
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* get home distance state */
bool home_dist_ok = check_dist_1wp();
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = !home_dist_ok;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available, switch to loiter */
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished");
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: no mission available");
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
*/
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
}
_mission_type = MISSION_TYPE_NONE;
@@ -397,7 +407,7 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
@@ -483,7 +493,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read");
"ERROR waypoint could not be read");
return false;
}
@@ -502,7 +512,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP waypoint could not be written");
"ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -511,8 +521,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed");
mavlink_log_critical(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -526,7 +536,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP is cycling, giving up");
"ERROR DO JUMP is cycling, giving up");
return false;
}
src/modules/navigator/mission_params.c
+2 -2
View File
@@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
* waypoint is more distant than MIS_DIS_1WP from the current position.
*
* @min 0
* @max 250
* @max 1000
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
src/modules/sdlog2/sdlog2.c
+2 -4
View File
@@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
src/modules/sdlog2/sdlog2_messages.h
+1 -3
View File
@@ -333,13 +333,11 @@ struct log_GS1B_s {
#define LOG_TECS_MSG 30
struct log_TECS_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
@@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */
src/modules/systemlib/circuit_breaker.c
+12
View File
@@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
*/
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
/**
* Circuit breaker for airspeed sensor
*
* Setting this parameter to 162128 will disable the check for an airspeed sensor.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 162128
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
src/modules/systemlib/circuit_breaker.h
+1
View File
@@ -52,6 +52,7 @@
#define CBRK_SUPPLY_CHK_KEY 894281
#define CBRK_RATE_CTRL_KEY 140253
#define CBRK_IO_SAFETY_KEY 22027
#define CBRK_AIRSPD_CHK_KEY 162128
#include <stdbool.h>
src/modules/uORB/topics/position_setpoint_triplet.h
+4 -4
View File
@@ -60,7 +60,7 @@ enum SETPOINT_TYPE
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */
SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
};
struct position_setpoint_s
@@ -71,9 +71,9 @@ struct position_setpoint_s
float y; /**< local position setpoint in m in NED */
float z; /**< local position setpoint in m in NED */
bool position_valid; /**< true if local position setpoint valid */
float vx; /**< local velocity setpoint in m in NED */
float vy; /**< local velocity setpoint in m in NED */
float vz; /**< local velocity setpoint in m in NED */
float vx; /**< local velocity setpoint in m/s in NED */
float vy; /**< local velocity setpoint in m/s in NED */
float vz; /**< local velocity setpoint in m/s in NED */
bool velocity_valid; /**< true if local velocity setpoint valid */
double lat; /**< latitude, in deg */
double lon; /**< longitude, in deg */
src/modules/uORB/topics/tecs_status.h
+6 -6
View File
@@ -51,11 +51,13 @@
*/
typedef enum {
TECS_MODE_NORMAL,
TECS_MODE_NORMAL = 0,
TECS_MODE_UNDERSPEED,
TECS_MODE_TAKEOFF,
TECS_MODE_LAND,
TECS_MODE_LAND_THROTTLELIM
TECS_MODE_LAND_THROTTLELIM,
TECS_MODE_BAD_DESCENT,
TECS_MODE_CLIMBOUT
} tecs_mode;
/**
@@ -65,14 +67,12 @@ struct tecs_status_s {
uint64_t timestamp; /**< timestamp, in microseconds since system start */
float altitudeSp;
float altitude;
float altitudeFiltered;
float altitude_filtered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeed_filtered;
float airspeedDerivativeSp;
float airspeedDerivative;
src/modules/uORB/topics/vehicle_global_position.h
+2 -2
View File
@@ -70,8 +70,8 @@ struct vehicle_global_position_s {
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float eph;
float epv;
float eph; /**< Standard deviation of position estimate horizontally */
float epv; /**< Standard deviation of position vertically */
};
/**
src/modules/uORB/topics/vehicle_local_position_setpoint.h
+2 -1
View File
@@ -50,11 +50,12 @@
*/
struct vehicle_local_position_setpoint_s {
uint64_t timestamp; /**< timestamp of the setpoint */
float x; /**< in meters NED */
float y; /**< in meters NED */
float z; /**< in meters NED */
float yaw; /**< in radians NED -PI..+PI */
}; /**< Local position in NED frame to go to */
}; /**< Local position in NED frame */
/**
* @}
src/modules/uORB/topics/vehicle_status.h
+1
View File
@@ -226,6 +226,7 @@ struct vehicle_status_s {
uint16_t errors_count4;
bool circuit_breaker_engaged_power_check;
bool circuit_breaker_engaged_airspd_check;
};
/**
src/modules/uavcan/module.mk
-4
View File
@@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM
#
# Invoke DSDL compiler
# TODO: Add make target for this, or invoke dsdlc manually.
# The second option assumes that the generated headers shall be saved
# under the version control, which may be undesirable.
# The first option requires any Python and the Python Mako library for the sources to be built.
#
$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
INCLUDE_DIRS += dsdlc_generated
src/modules/uavcan/uavcan_main.cpp
+63 -36
View File
@@ -39,6 +39,8 @@
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
@@ -173,6 +175,41 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
return OK;
}
void UavcanNode::fill_node_info()
{
/* software version */
uavcan::protocol::SoftwareVersion swver;
// Extracting the first 8 hex digits of FW_GIT and converting them to int
char fw_git_short[9] = {};
std::memmove(fw_git_short, FW_GIT, 8);
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
_node.setSoftwareVersion(swver);
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
_node.setHardwareVersion(hwver);
}
int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
@@ -183,6 +220,13 @@ int UavcanNode::init(uavcan::NodeID node_id)
if (ret != OK)
return ret;
_node.setName("org.pixhawk.pixhawk");
_node.setNodeID(node_id);
fill_node_info();
/* initializing the bridges UAVCAN <--> uORB */
ret = _esc_controller.init();
if (ret < 0)
return ret;
@@ -191,20 +235,6 @@ int UavcanNode::init(uavcan::NodeID node_id)
if (ret < 0)
return ret;
uavcan::protocol::SoftwareVersion swver;
swver.major = 12; // TODO fill version info
swver.minor = 34;
_node.setSoftwareVersion(swver);
uavcan::protocol::HardwareVersion hwver;
hwver.major = 42; // TODO fill version info
hwver.minor = 42;
_node.setHardwareVersion(hwver);
_node.setName("org.pixhawk"); // Huh?
_node.setNodeID(node_id);
return _node.start();
}
@@ -223,7 +253,6 @@ int UavcanNode::run()
// XXX figure out the output count
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
actuator_outputs_s outputs;
@@ -243,21 +272,23 @@ int UavcanNode::run()
_node.setStatusOk();
while (!_task_should_exit) {
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds_num = 0;
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
while (!_task_should_exit) {
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@@ -271,7 +302,7 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 0;
unsigned poll_id = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
@@ -282,12 +313,7 @@ int UavcanNode::run()
}
}
if (!controls_updated) {
// timeout: no control data, switch to failsafe values
// XXX trigger failsafe
}
//can we mix?
// can we mix?
if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
@@ -387,7 +413,8 @@ UavcanNode::subscribe()
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
// the first fd used by CAN
_poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@@ -493,8 +520,8 @@ UavcanNode::print_info()
warnx("not running, start first");
}
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
}
/*
src/modules/uavcan/uavcan_main.hpp
+1
View File
@@ -94,6 +94,7 @@ public:
static UavcanNode* instance() { return _instance; }
private:
void fill_node_info();
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
src/systemcmds/nshterm/module.mk
+1 -1
View File
@@ -38,4 +38,4 @@
MODULE_COMMAND = nshterm
SRCS = nshterm.c
MODULE_STACKSIZE = 1200
MODULE_STACKSIZE = 1400
uavcan
+1 -1
Submodule uavcan updated: d84fc8a846...6980ee8240