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

Merge branch 'master' into seatbelt_multirotor

This commit is contained in:
Anton Babushkin 2013-06-10 16:21:10 +04:00
parent 4bf49cfc35 42ce3112ad
commit afb34950a3
37 changed files with 3632 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ROMFS/px4fmu_common/mixers/IO_pass.mix Normal file
View File

@ -0,0 +1,38 @@
Passthrough mixer for PX4IO
============================
This file defines passthrough mixers suitable for testing.
Channel group 0, channels 0-7 are passed directly through to the outputs.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
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

293
Tools/sdlog2_dump.py Normal file
View File

@ -0,0 +1,293 @@
#!/usr/bin/env python
"""Dump binary log generated by sdlog2 or APM as CSV
Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]
-v Use plain debug output instead of CSV.
-e Recover from errors.
-d Use "delimiter" in CSV. Default is ",".
-n Use "null" as placeholder for empty values in CSV. Default is empty.
-m MSG[.field1,field2,...]
Dump only messages of specified type, and only specified fields.
Multiple -m options allowed."""
__author__ = "Anton Babushkin"
__version__ = "1.2"
import struct, sys
class SDLog2Parser:
BLOCK_SIZE = 8192
MSG_HEADER_LEN = 3
MSG_HEAD1 = 0xA3
MSG_HEAD2 = 0x95
MSG_FORMAT_PACKET_LEN = 89
MSG_FORMAT_STRUCT = "BB4s16s64s"
MSG_TYPE_FORMAT = 0x80
FORMAT_TO_STRUCT = {
"b": ("b", None),
"B": ("B", None),
"h": ("h", None),
"H": ("H", None),
"i": ("i", None),
"I": ("I", None),
"f": ("f", None),
"n": ("4s", None),
"N": ("16s", None),
"Z": ("64s", None),
"c": ("h", 0.01),
"C": ("H", 0.01),
"e": ("i", 0.01),
"E": ("I", 0.01),
"L": ("i", 0.0000001),
"M": ("b", None),
"q": ("q", None),
"Q": ("Q", None),
}
__csv_delim = ","
__csv_null = ""
__msg_filter = []
__time_msg = None
__debug_out = False
__correct_errors = False
def __init__(self):
return
def reset(self):
self.__msg_descrs = {} # message descriptions by message type map
self.__msg_labels = {} # message labels by message name map
self.__msg_names = [] # message names in the same order as FORMAT messages
self.__buffer = "" # buffer for input binary data
self.__ptr = 0 # read pointer in buffer
self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label"
self.__csv_data = {} # current values for all columns
self.__csv_updated = False
self.__msg_filter_map = {} # filter in form of map, with '*" expanded to full list of fields
def setCSVDelimiter(self, csv_delim):
self.__csv_delim = csv_delim
def setCSVNull(self, csv_null):
self.__csv_null = csv_null
def setMsgFilter(self, msg_filter):
self.__msg_filter = msg_filter
def setTimeMsg(self, time_msg):
self.__time_msg = time_msg
def setDebugOut(self, debug_out):
self.__debug_out = debug_out
def setCorrectErrors(self, correct_errors):
self.__correct_errors = correct_errors
def process(self, fn):
self.reset()
if self.__debug_out:
# init __msg_filter_map
for msg_name, show_fields in self.__msg_filter:
self.__msg_filter_map[msg_name] = show_fields
first_data_msg = True
f = open(fn, "r")
bytes_read = 0
while True:
chunk = f.read(self.BLOCK_SIZE)
if len(chunk) == 0:
break
self.__buffer = self.__buffer[self.__ptr:] + chunk
self.__ptr = 0
while self.__bytesLeft() >= self.MSG_HEADER_LEN:
head1 = ord(self.__buffer[self.__ptr])
head2 = ord(self.__buffer[self.__ptr+1])
if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2):
if self.__correct_errors:
self.__ptr += 1
continue
else:
raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2))
msg_type = ord(self.__buffer[self.__ptr+2])
if msg_type == self.MSG_TYPE_FORMAT:
# parse FORMAT message
if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN:
break
self.__parseMsgDescr()
else:
# parse data message
msg_descr = self.__msg_descrs[msg_type]
if msg_descr == None:
raise Exception("Unknown msg type: %i" % msg_type)
msg_length = msg_descr[0]
if self.__bytesLeft() < msg_length:
break
if first_data_msg:
# build CSV columns and init data map
self.__initCSV()
first_data_msg = False
self.__parseMsg(msg_descr)
bytes_read += self.__ptr
if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
self.__printCSVRow()
f.close()
def __bytesLeft(self):
return len(self.__buffer) - self.__ptr
def __filterMsg(self, msg_name):
show_fields = "*"
if len(self.__msg_filter_map) > 0:
show_fields = self.__msg_filter_map.get(msg_name)
return show_fields
def __initCSV(self):
if len(self.__msg_filter) == 0:
for msg_name in self.__msg_names:
self.__msg_filter.append((msg_name, "*"))
for msg_name, show_fields in self.__msg_filter:
if show_fields == "*":
show_fields = self.__msg_labels.get(msg_name, [])
self.__msg_filter_map[msg_name] = show_fields
for field in show_fields:
full_label = msg_name + "." + field
self.__csv_columns.append(full_label)
self.__csv_data[full_label] = None
print self.__csv_delim.join(self.__csv_columns)
def __printCSVRow(self):
s = []
for full_label in self.__csv_columns:
v = self.__csv_data[full_label]
if v == None:
v = self.__csv_null
else:
v = str(v)
s.append(v)
print self.__csv_delim.join(s)
def __parseMsgDescr(self):
data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
msg_type = data[0]
if msg_type != self.MSG_TYPE_FORMAT:
msg_length = data[1]
msg_name = data[2].strip("\0")
msg_format = data[3].strip("\0")
msg_labels = data[4].strip("\0").split(",")
# Convert msg_format to struct.unpack format string
msg_struct = ""
msg_mults = []
for c in msg_format:
try:
f = self.FORMAT_TO_STRUCT[c]
msg_struct += f[0]
msg_mults.append(f[1])
except KeyError as e:
raise Exception("Unsupported format char: %s in message %s (%i)" % (c, msg_name, msg_type))
msg_struct = "<" + msg_struct # force little-endian
self.__msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults)
self.__msg_labels[msg_name] = msg_labels
self.__msg_names.append(msg_name)
if self.__debug_out:
if self.__filterMsg(msg_name) != None:
print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)
self.__ptr += self.MSG_FORMAT_PACKET_LEN
def __parseMsg(self, msg_descr):
msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr
if not self.__debug_out and self.__time_msg != None and msg_name == self.__time_msg and self.__csv_updated:
self.__printCSVRow()
self.__csv_updated = False
show_fields = self.__filterMsg(msg_name)
if (show_fields != None):
data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
for i in xrange(len(data)):
if type(data[i]) is str:
data[i] = data[i].strip("\0")
m = msg_mults[i]
if m != None:
data[i] = data[i] * m
if self.__debug_out:
s = []
for i in xrange(len(data)):
label = msg_labels[i]
if show_fields == "*" or label in show_fields:
s.append(label + "=" + str(data[i]))
print "MSG %s: %s" % (msg_name, ", ".join(s))
else:
# update CSV data buffer
for i in xrange(len(data)):
label = msg_labels[i]
if label in show_fields:
self.__csv_data[msg_name + "." + label] = data[i]
if self.__time_msg != None and msg_name != self.__time_msg:
self.__csv_updated = True
if self.__time_msg == None:
self.__printCSVRow()
self.__ptr += msg_length
def _main():
if len(sys.argv) < 2:
print "Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n"
print "\t-v\tUse plain debug output instead of CSV.\n"
print "\t-e\tRecover from errors.\n"
print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n"
print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n"
print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed."
print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n"
return
fn = sys.argv[1]
debug_out = False
correct_errors = False
msg_filter = []
csv_null = ""
csv_delim = ","
time_msg = None
opt = None
for arg in sys.argv[2:]:
if opt != None:
if opt == "d":
csv_delim = arg
elif opt == "n":
csv_null = arg
elif opt == "t":
time_msg = arg
elif opt == "m":
show_fields = "*"
a = arg.split(".")
if len(a) > 1:
show_fields = a[1].split(",")
msg_filter.append((a[0], show_fields))
opt = None
else:
if arg == "-v":
debug_out = True
elif arg == "-e":
correct_errors = True
elif arg == "-d":
opt = "d"
elif arg == "-n":
opt = "n"
elif arg == "-m":
opt = "m"
elif arg == "-t":
opt = "t"
if csv_delim == "\\t":
csv_delim = "\t"
parser = SDLog2Parser()
parser.setCSVDelimiter(csv_delim)
parser.setCSVNull(csv_null)
parser.setMsgFilter(msg_filter)
parser.setTimeMsg(time_msg)
parser.setDebugOut(debug_out)
parser.setCorrectErrors(correct_errors)
parser.process(fn)
if __name__ == "__main__":
_main()

4
makefiles/config_px4fmu_default.mk
View File

@ -62,7 +62,8 @@ MODULES += modules/gpio_led
# Estimation modules (EKF / other filters) # Estimation modules (EKF / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/position_estimator_mc MODULES += modules/attitude_estimator_so3_comp
#MODULES += modules/position_estimator_mc
MODULES += modules/position_estimator MODULES += modules/position_estimator
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
@ -80,6 +81,7 @@ MODULES += modules/multirotor_pos_control
# Logging # Logging
# #
MODULES += modules/sdlog MODULES += modules/sdlog
MODULES += modules/sdlog2
# #
# Library modules # Library modules

12
makefiles/firmware.mk
View File

@ -176,6 +176,12 @@ GLOBAL_DEPS += $(MAKEFILE_LIST)
# #
EXTRA_CLEANS = EXTRA_CLEANS =
################################################################################
# NuttX libraries and paths
################################################################################
include $(PX4_MK_DIR)/nuttx.mk
################################################################################ ################################################################################
# Modules # Modules
################################################################################ ################################################################################
@ -296,12 +302,6 @@ $(LIBRARY_CLEANS):
LIBRARY_MK=$(mkfile) \ LIBRARY_MK=$(mkfile) \
clean clean
################################################################################
# NuttX libraries and paths
################################################################################
include $(PX4_MK_DIR)/nuttx.mk
################################################################################ ################################################################################
# ROMFS generation # ROMFS generation
################################################################################ ################################################################################

6
makefiles/nuttx.mk
View File

@ -69,10 +69,14 @@ INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
LIB_DIRS += $(NUTTX_EXPORT_DIR)libs LIB_DIRS += $(NUTTX_EXPORT_DIR)libs
LIBS += -lapps -lnuttx LIBS += -lapps -lnuttx
LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \ NUTTX_LIBS = $(NUTTX_EXPORT_DIR)libs/libapps.a \
$(NUTTX_EXPORT_DIR)libs/libnuttx.a $(NUTTX_EXPORT_DIR)libs/libnuttx.a
LINK_DEPS += $(NUTTX_LIBS)
$(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE) $(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE)
@$(ECHO) %% Unpacking $(NUTTX_ARCHIVE) @$(ECHO) %% Unpacking $(NUTTX_ARCHIVE)
$(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) $(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE)
$(Q) $(TOUCH) $@ $(Q) $(TOUCH) $@
$(LDSCRIPT): $(NUTTX_CONFIG_HEADER)
$(NUTTX_LIBS): $(NUTTX_CONFIG_HEADER)

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

@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump
# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
MAXOPTIMIZATION = -O3 MAXOPTIMIZATION ?= -O3
# Base CPU flags for each of the supported architectures. # Base CPU flags for each of the supported architectures.
# #
@ -70,6 +70,14 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
-march=armv7-m \ -march=armv7-m \
-mfloat-abi=soft -mfloat-abi=soft
ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
# Pick the right set of flags for the architecture. # Pick the right set of flags for the architecture.
# #
ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
@ -91,8 +99,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
# enable precise stack overflow tracking # enable precise stack overflow tracking
# note - requires corresponding support in NuttX # note - requires corresponding support in NuttX
INSTRUMENTATIONDEFINES = -finstrument-functions \ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
-ffixed-r10
# Language-specific flags # Language-specific flags
# #
ARCHCFLAGS = -std=gnu99 ARCHCFLAGS = -std=gnu99

2
nuttx/arch/arm/src/armv7-m/Toolchain.defs
View File

@ -243,7 +243,7 @@ endif
ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),GNU_EABI) ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),GNU_EABI)
CROSSDEV ?= arm-none-eabi- CROSSDEV ?= arm-none-eabi-
ARCROSSDEV ?= arm-none-eabi- ARCROSSDEV ?= arm-none-eabi-
MAXOPTIMIZATION = -O3 MAXOPTIMIZATION ?= -O3
ifeq ($(CONFIG_ARCH_CORTEXM4),y) ifeq ($(CONFIG_ARCH_CORTEXM4),y)
ifeq ($(CONFIG_ARCH_FPU),y) ifeq ($(CONFIG_ARCH_FPU),y)
ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard

2
nuttx/configs/px4fmu/common/Make.defs
View File

@ -58,7 +58,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump OBJDUMP = $(CROSSDEV)objdump
MAXOPTIMIZATION = -O3 MAXOPTIMIZATION ?= -O3
ARCHCPUFLAGS = -mcpu=cortex-m4 \ ARCHCPUFLAGS = -mcpu=cortex-m4 \
-mthumb \ -mthumb \
-march=armv7e-m \ -march=armv7e-m \

12
nuttx/configs/px4fmu/nsh/defconfig Executable file → Normal file
View File

@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y
CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_SERIAL_CONSOLE_REINIT=y
CONFIG_STANDARD_SERIAL=y CONFIG_STANDARD_SERIAL=y
CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART1_SERIAL_CONSOLE=n
CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_UART4_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n
@ -561,7 +561,7 @@ CONFIG_START_MONTH=1
CONFIG_START_DAY=1 CONFIG_START_DAY=1
CONFIG_GREGORIAN_TIME=n CONFIG_GREGORIAN_TIME=n
CONFIG_JULIAN_TIME=n CONFIG_JULIAN_TIME=n
CONFIG_DEV_CONSOLE=y CONFIG_DEV_CONSOLE=n
CONFIG_DEV_LOWCONSOLE=n CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=y CONFIG_PRIORITY_INHERITANCE=y
@ -717,7 +717,7 @@ CONFIG_ARCH_BZERO=n
# zero for all dynamic allocations. # zero for all dynamic allocations.
# #
CONFIG_MAX_TASKS=32 CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=8 CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4 CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=25 CONFIG_NFILE_STREAMS=25
@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512
# Size of the serial receive/transmit buffers. Default 256. # Size of the serial receive/transmit buffers. Default 256.
# #
CONFIG_CDCACM=y CONFIG_CDCACM=y
CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_CONSOLE=y
#CONFIG_CDCACM_EP0MAXPACKET #CONFIG_CDCACM_EP0MAXPACKET
CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN=1
#CONFIG_CDCACM_EPINTIN_FSSIZE #CONFIG_CDCACM_EPINTIN_FSSIZE
@ -955,7 +955,7 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6"
# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer # CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
# CONFIG_NSH_STRERROR - Use strerror(errno) # CONFIG_NSH_STRERROR - Use strerror(errno)
# CONFIG_NSH_LINELEN - Maximum length of one command line # CONFIG_NSH_LINELEN - Maximum length of one command line
# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line # CONFIG_NSH_MAXARGUMENTS - Maximum number of arguments for command line
# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi # CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
# CONFIG_NSH_DISABLESCRIPT - Disable scripting support # CONFIG_NSH_DISABLESCRIPT - Disable scripting support
# CONFIG_NSH_DISABLEBG - Disable background commands # CONFIG_NSH_DISABLEBG - Disable background commands
@ -988,7 +988,7 @@ CONFIG_NSH_BUILTIN_APPS=y
CONFIG_NSH_FILEIOSIZE=512 CONFIG_NSH_FILEIOSIZE=512
CONFIG_NSH_STRERROR=y CONFIG_NSH_STRERROR=y
CONFIG_NSH_LINELEN=128 CONFIG_NSH_LINELEN=128
CONFIG_NSH_MAX_ARGUMENTS=12 CONFIG_NSH_MAXARGUMENTS=12
CONFIG_NSH_NESTDEPTH=8 CONFIG_NSH_NESTDEPTH=8
CONFIG_NSH_DISABLESCRIPT=n CONFIG_NSH_DISABLESCRIPT=n
CONFIG_NSH_DISABLEBG=n CONFIG_NSH_DISABLEBG=n

2
nuttx/configs/px4io/common/Make.defs
View File

@ -58,7 +58,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump OBJDUMP = $(CROSSDEV)objdump
MAXOPTIMIZATION = -O3 MAXOPTIMIZATION ?= -O3
ARCHCPUFLAGS = -mcpu=cortex-m3 \ ARCHCPUFLAGS = -mcpu=cortex-m3 \
-mthumb \ -mthumb \
-march=armv7-m -march=armv7-m

132
src/drivers/hott_telemetry/hott_telemetry_main.c
View File

@ -53,6 +53,7 @@
#include <termios.h> #include <termios.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <unistd.h> #include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include "messages.h" #include "messages.h"
@ -60,56 +61,44 @@
static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */ static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */ static int deamon_task; /**< Handle of deamon task / thread */
static char *daemon_name = "hott_telemetry"; static const char daemon_name[] = "hott_telemetry";
static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]"; static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d <device>]";
/* A little console messaging experiment - console helper macro */
#define FATAL_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
#define ERROR_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
#define INFO_MSG(_msg) printf("[%s] %s\n", daemon_name, _msg);
/** /**
* Deamon management function. * Deamon management function.
*/ */
__EXPORT int hott_telemetry_main(int argc, char *argv[]); __EXPORT int hott_telemetry_main(int argc, char *argv[]);
/** /**
* Mainloop of deamon. * Mainloop of daemon.
*/ */
int hott_telemetry_thread_main(int argc, char *argv[]); int hott_telemetry_thread_main(int argc, char *argv[]);
static int read_data(int uart, int *id); static int recv_req_id(int uart, uint8_t *id);
static int send_data(int uart, uint8_t *buffer, int size); static int send_data(int uart, uint8_t *buffer, size_t size);
static int open_uart(const char *device, struct termios *uart_config_original) static int
open_uart(const char *device, struct termios *uart_config_original)
{ {
/* baud rate */ /* baud rate */
int speed = B19200; static const speed_t speed = B19200;
int uart;
/* open uart */ /* open uart */
uart = open(device, O_RDWR | O_NOCTTY); const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) { if (uart < 0) {
char msg[80]; err(1, "Error opening port: %s", device);
sprintf(msg, "Error opening port: %s\n", device);
FATAL_MSG(msg);
} }
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */ /* Back up the original uart configuration to restore it after exit */
char msg[80]; int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
close(uart); close(uart);
FATAL_MSG(msg); err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
} }
/* Fill the struct for the new configuration */ /* Fill the struct for the new configuration */
struct termios uart_config;
tcgetattr(uart, &uart_config); tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */ /* Clear ONLCR flag (which appends a CR for every LF) */
@ -117,16 +106,14 @@ static int open_uart(const char *device, struct termios *uart_config_original)
/* Set baud rate */ /* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
device, termios_state);
close(uart); close(uart);
FATAL_MSG(msg); err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
} }
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
close(uart); close(uart);
FATAL_MSG(msg); err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
} }
/* Activate single wire mode */ /* Activate single wire mode */
@ -135,39 +122,41 @@ static int open_uart(const char *device, struct termios *uart_config_original)
return uart; return uart;
} }
int read_data(int uart, int *id) int
recv_req_id(int uart, uint8_t *id)
{ {
const int timeout = 1000; static const int timeout_ms = 1000; // TODO make it a define
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
char mode; uint8_t mode;
if (poll(fds, 1, timeout) > 0) { if (poll(fds, 1, timeout_ms) > 0) {
/* Get the mode: binary or text */ /* Get the mode: binary or text */
read(uart, &mode, 1); read(uart, &mode, sizeof(mode));
/* Read the device ID being polled */
read(uart, id, 1);
/* if we have a binary mode request */ /* if we have a binary mode request */
if (mode != BINARY_MODE_REQUEST_ID) { if (mode != BINARY_MODE_REQUEST_ID) {
return ERROR; return ERROR;
} }
/* Read the device ID being polled */
read(uart, id, sizeof(*id));
} else { } else {
ERROR_MSG("UART timeout on TX/RX port"); warnx("UART timeout on TX/RX port");
return ERROR; return ERROR;
} }
return OK; return OK;
} }
int send_data(int uart, uint8_t *buffer, int size) int
send_data(int uart, uint8_t *buffer, size_t size)
{ {
usleep(POST_READ_DELAY_IN_USECS); usleep(POST_READ_DELAY_IN_USECS);
uint16_t checksum = 0; uint16_t checksum = 0;
for (int i = 0; i < size; i++) { for (size_t i = 0; i < size; i++) {
if (i == size - 1) { if (i == size - 1) {
/* Set the checksum: the first uint8_t is taken as the checksum. */ /* Set the checksum: the first uint8_t is taken as the checksum. */
buffer[i] = checksum & 0xff; buffer[i] = checksum & 0xff;
@ -176,7 +165,7 @@ int send_data(int uart, uint8_t *buffer, int size)
checksum += buffer[i]; checksum += buffer[i];
} }
write(uart, &buffer[i], 1); write(uart, &buffer[i], sizeof(buffer[i]));
/* Sleep before sending the next byte. */ /* Sleep before sending the next byte. */
usleep(POST_WRITE_DELAY_IN_USECS); usleep(POST_WRITE_DELAY_IN_USECS);
@ -190,13 +179,14 @@ int send_data(int uart, uint8_t *buffer, int size)
return OK; return OK;
} }
int hott_telemetry_thread_main(int argc, char *argv[]) int
hott_telemetry_thread_main(int argc, char *argv[])
{ {
INFO_MSG("starting"); warnx("starting");
thread_running = true; thread_running = true;
char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
/* read commandline arguments */ /* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) { for (int i = 0; i < argc && argv[i]; i++) {
@ -206,45 +196,55 @@ int hott_telemetry_thread_main(int argc, char *argv[])
} else { } else {
thread_running = false; thread_running = false;
ERROR_MSG("missing parameter to -d"); errx(1, "missing parameter to -d\n%s", commandline_usage);
ERROR_MSG(commandline_usage);
exit(1);
} }
} }
} }
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
struct termios uart_config_original; struct termios uart_config_original;
int uart = open_uart(device, &uart_config_original); const int uart = open_uart(device, &uart_config_original);
if (uart < 0) { if (uart < 0) {
ERROR_MSG("Failed opening HoTT UART, exiting."); errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false; thread_running = false;
exit(ERROR);
} }
messages_init(); messages_init();
uint8_t buffer[MESSAGE_BUFFER_SIZE]; uint8_t buffer[MESSAGE_BUFFER_SIZE];
int size = 0; size_t size = 0;
int id = 0; uint8_t id = 0;
bool connected = true;
while (!thread_should_exit) { while (!thread_should_exit) {
if (read_data(uart, &id) == OK) { if (recv_req_id(uart, &id) == OK) {
if (!connected) {
connected = true;
warnx("OK");
}
switch (id) { switch (id) {
case ELECTRIC_AIR_MODULE: case EAM_SENSOR_ID:
build_eam_response(buffer, &size); build_eam_response(buffer, &size);
break; break;
case GPS_SENSOR_ID:
build_gps_response(buffer, &size);
break;
default: default:
continue; // Not a module we support. continue; // Not a module we support.
} }
send_data(uart, buffer, size); send_data(uart, buffer, size);
} else {
connected = false;
warnx("syncing");
} }
} }
INFO_MSG("exiting"); warnx("exiting");
close(uart); close(uart);
@ -256,23 +256,22 @@ int hott_telemetry_thread_main(int argc, char *argv[])
/** /**
* Process command line arguments and tart the daemon. * Process command line arguments and tart the daemon.
*/ */
int hott_telemetry_main(int argc, char *argv[]) int
hott_telemetry_main(int argc, char *argv[])
{ {
if (argc < 1) { if (argc < 1) {
ERROR_MSG("missing command"); errx(1, "missing command\n%s", commandline_usage);
ERROR_MSG(commandline_usage);
exit(1);
} }
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
INFO_MSG("deamon already running"); warnx("deamon already running");
exit(0); exit(0);
} }
thread_should_exit = false; thread_should_exit = false;
deamon_task = task_spawn("hott_telemetry", deamon_task = task_spawn(daemon_name,
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40, SCHED_PRIORITY_MAX - 40,
2048, 2048,
@ -288,19 +287,14 @@ int hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
INFO_MSG("daemon is running"); warnx("daemon is running");
} else { } else {
INFO_MSG("daemon not started"); warnx("daemon not started");
} }
exit(0); exit(0);
} }
ERROR_MSG("unrecognized command"); errx(1, "unrecognized command\n%s", commandline_usage);
ERROR_MSG(commandline_usage);
exit(1);
} }

176
src/drivers/hott_telemetry/messages.c
View File

@ -1,7 +1,7 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com> * Author: @author Simon Wilks <sjwilks@gmail.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -34,30 +34,44 @@
/** /**
* @file messages.c * @file messages.c
* @author Simon Wilks <sjwilks@gmail.com> *
*/ */
#include "messages.h" #include "messages.h"
#include <math.h>
#include <stdio.h>
#include <string.h> #include <string.h>
#include <systemlib/systemlib.h> #include <systemlib/geo/geo.h>
#include <unistd.h> #include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
static int airspeed_sub = -1;
static int battery_sub = -1; static int battery_sub = -1;
static int gps_sub = -1;
static int home_sub = -1;
static int sensor_sub = -1; static int sensor_sub = -1;
void messages_init(void) static bool home_position_set = false;
static double home_lat = 0.0d;
static double home_lon = 0.0d;
void
messages_init(void)
{ {
battery_sub = orb_subscribe(ORB_ID(battery_status)); battery_sub = orb_subscribe(ORB_ID(battery_status));
gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
home_sub = orb_subscribe(ORB_ID(home_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
} }
void build_eam_response(uint8_t *buffer, int *size) void
build_eam_response(uint8_t *buffer, size_t *size)
{ {
/* get a local copy of the current sensor values */ /* get a local copy of the current sensor values */
struct sensor_combined_s raw; struct sensor_combined_s raw;
@ -74,26 +88,144 @@ void build_eam_response(uint8_t *buffer, int *size)
memset(&msg, 0, *size); memset(&msg, 0, *size);
msg.start = START_BYTE; msg.start = START_BYTE;
msg.eam_sensor_id = ELECTRIC_AIR_MODULE; msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_id = EAM_SENSOR_ID; msg.sensor_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
msg.temperature2 = TEMP_ZERO_CELSIUS; msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the current sensor values */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE; msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size); memcpy(buffer, &msg, *size);
} }
void
build_gps_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* get a local copy of the battery data */
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
struct gps_module_msg msg = { 0 };
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
msg.gps_num_sat = gps.satellites_visible;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
msg.gps_fix = (uint8_t)(gps.fix_type + 48);
/* No point collecting more data if we don't have a 3D fix yet */
if (gps.fix_type > 2) {
/* Current flight direction */
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
/* GPS speed */
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat))*1e-7d;
/* Set the N or S specifier */
msg.latitude_ns = 0;
if (lat < 0) {
msg.latitude_ns = 1;
lat = abs(lat);
}
int deg;
int min;
int sec;
convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
uint16_t lat_min = (uint16_t)(deg * 100 + min);
msg.latitude_min_L = (uint8_t)lat_min & 0xff;
msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
uint16_t lat_sec = (uint16_t)(sec);
msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
/* Get longitude in degrees, minutes and seconds */
double lon = ((double)(gps.lon))*1e-7d;
/* Set the E or W specifier */
msg.longitude_ew = 0;
if (lon < 0) {
msg.longitude_ew = 1;
lon = abs(lon);
}
convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
uint16_t lon_min = (uint16_t)(deg * 100 + min);
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
uint16_t lon_sec = (uint16_t)(sec);
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* Get any (and probably only ever one) home_sub postion report */
bool updated;
orb_check(home_sub, &updated);
if (updated) {
/* get a local copy of the home position data */
struct home_position_s home;
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), home_sub, &home);
home_lat = ((double)(home.lat))*1e-7d;
home_lon = ((double)(home.lon))*1e-7d;
home_position_set = true;
}
/* Distance from home */
if (home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
msg.distance_L = (uint8_t)dist & 0xff;
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
/* Direction back to home */
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
msg.home_direction = (uint8_t)bearing >> 1;
}
}
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
void
convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
{
*deg = (int)val;
double delta = val - *deg;
const double min_d = delta * 60.0d;
*min = (int)min_d;
delta = min_d - *min;
*sec = (int)(delta * 10000.0d);
}

90
src/drivers/hott_telemetry/messages.h
View File

@ -44,11 +44,6 @@
#include <stdlib.h> #include <stdlib.h>
/* The buffer size used to store messages. This must be at least as big as the number of
* fields in the largest message struct.
*/
#define MESSAGE_BUFFER_SIZE 50
/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. /* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
* Note that the value specified here is lower than 5000 (5ms) as time is lost constucting * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
* the message after the read which takes some milliseconds. * the message after the read which takes some milliseconds.
@ -66,13 +61,13 @@
#define TEMP_ZERO_CELSIUS 0x14 #define TEMP_ZERO_CELSIUS 0x14
/* Electric Air Module (EAM) constants. */ /* Electric Air Module (EAM) constants. */
#define ELECTRIC_AIR_MODULE 0x8e #define EAM_SENSOR_ID 0x8e
#define EAM_SENSOR_ID 0xe0 #define EAM_SENSOR_TEXT_ID 0xe0
/* The Electric Air Module message. */ /* The Electric Air Module message. */
struct eam_module_msg { struct eam_module_msg {
uint8_t start; /**< Start byte */ uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor ID */ uint8_t eam_sensor_id; /**< EAM sensor */
uint8_t warning; uint8_t warning;
uint8_t sensor_id; /**< Sensor ID, why different? */ uint8_t sensor_id; /**< Sensor ID, why different? */
uint8_t alarm_inverse1; uint8_t alarm_inverse1;
@ -118,7 +113,84 @@ struct eam_module_msg {
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
}; };
/**
* The maximum buffer size required to store a HoTT message.
*/
#define MESSAGE_BUFFER_SIZE sizeof(union { \
struct eam_module_msg eam; \
})
/* GPS sensor constants. */
#define GPS_SENSOR_ID 0x8A
#define GPS_SENSOR_TEXT_ID 0xA0
/**
* The GPS sensor message
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
*/
struct gps_module_msg {
uint8_t start; /**< Start byte */
uint8_t sensor_id; /**< GPS sensor ID*/
uint8_t warning; /**< Byte 3: 0…= warning beeps */
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */
uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */
uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */
uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */
uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */
uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */
uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */
uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */
uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */
uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */
uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */
uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */
uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */
uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */
uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */
uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */
uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */
uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */
uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */
uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */
uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */
uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */
uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */
uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */
uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */
uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */
uint8_t vibration; /**< Byte 39: vibration (1 bytes) */
uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */
uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */
uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */
uint8_t version; /**< Byte 43: 00 version number */
uint8_t stop; /**< Byte 44: 0x7D Ende byte */
uint8_t checksum; /**< Byte 45: Parity Byte */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \
struct gps_module_msg gps; \
})
void messages_init(void); void messages_init(void);
void build_eam_response(uint8_t *buffer, int *size); void build_eam_response(uint8_t *buffer, size_t *size);
void build_gps_response(uint8_t *buffer, size_t *size);
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
#endif /* MESSAGES_H_ */ #endif /* MESSAGES_H_ */

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

@ -116,6 +116,14 @@ public:
*/ */
void set_battery_current_scaling(float amp_per_volt, float amp_bias); void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Push failsafe values to IO.
*
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/** /**
* Print the current status of IO * Print the current status of IO
*/ */
@ -326,11 +334,11 @@ PX4IO::PX4IO() :
_to_actuators_effective(0), _to_actuators_effective(0),
_to_outputs(0), _to_outputs(0),
_to_battery(0), _to_battery(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0), _battery_amp_bias(0),
_battery_mamphour_total(0), _battery_mamphour_total(0),
_battery_last_timestamp(0), _battery_last_timestamp(0)
_primary_pwm_device(false)
{ {
/* we need this potentially before it could be set in task_main */ /* we need this potentially before it could be set in task_main */
g_dev = this; g_dev = this;
@ -689,6 +697,19 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
} }
int
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
int int
PX4IO::io_set_arming_state() PX4IO::io_set_arming_state()
{ {
@ -1250,7 +1271,7 @@ PX4IO::print_status()
printf("%u bytes free\n", printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags, flags,
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@ -1262,7 +1283,8 @@ PX4IO::print_status()
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
printf("alarms 0x%04x%s%s%s%s%s%s%s\n", printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
alarms, alarms,
@ -1280,7 +1302,7 @@ PX4IO::print_status()
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n", printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
(double)_battery_amp_per_volt, (double)_battery_amp_per_volt,
(double)_battery_amp_bias, (double)_battery_amp_bias,
(double)_battery_mamphour_total); (double)_battery_mamphour_total);
@ -1474,7 +1496,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case MIXERIOCLOADBUF: { case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg; const char *buf = (const char *)arg;
ret = mixer_send(buf, strnlen(buf, 1024)); ret = mixer_send(buf, strnlen(buf, 2048));
break; break;
} }
@ -1615,6 +1637,13 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0)) if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos"); err(1, "failed to arm servos");
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("Press CTRL-C or 'c' to abort.");
for (;;) { for (;;) {
/* sweep all servos between 1000..2000 */ /* sweep all servos between 1000..2000 */
@ -1649,6 +1678,16 @@ test(void)
if (value != servos[i]) if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
} }
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
warnx("User abort\n");
close(console);
exit(0);
}
}
} }
} }
@ -1718,6 +1757,41 @@ px4io_main(int argc, char *argv[])
exit(0); exit(0);
} }
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < 800 || failsafe[i] > 2200) {
errx(1, "value out of range of 800 < value < 2200. Aborting.");
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) { if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) { if (g_dev != nullptr) {
@ -1845,5 +1919,5 @@ px4io_main(int argc, char *argv[])
monitor(); monitor();
out: out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'"); errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
} }

54
src/examples/px4_daemon_app/px4_daemon_app.c
View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -33,27 +32,33 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file px4_deamon_app.c * @file px4_daemon_app.c
* Deamon application example for PX4 autopilot * daemon application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
#include <nuttx/sched.h>
#include <unistd.h> #include <unistd.h>
#include <stdio.h> #include <stdio.h>
static bool thread_should_exit = false; /**< Deamon exit flag */ #include <systemlib/systemlib.h>
static bool thread_running = false; /**< Deamon status flag */ #include <systemlib/err.h>
static int deamon_task; /**< Handle of deamon task / thread */
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
/** /**
* Deamon management function. * daemon management function.
*/ */
__EXPORT int px4_deamon_app_main(int argc, char *argv[]); __EXPORT int px4_daemon_app_main(int argc, char *argv[]);
/** /**
* Mainloop of deamon. * Mainloop of daemon.
*/ */
int px4_deamon_thread_main(int argc, char *argv[]); int px4_daemon_thread_main(int argc, char *argv[]);
/** /**
* Print the correct usage. * Print the correct usage.
@ -64,20 +69,19 @@ static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); warnx("%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
} }
/** /**
* The deamon app only briefly exists to start * The daemon app only briefly exists to start
* the background job. The stack size assigned in the * the background job. The stack size assigned in the
* Makefile does only apply to this management task. * Makefile does only apply to this management task.
* *
* The actual stack size should be set in the call * The actual stack size should be set in the call
* to task_create(). * to task_create().
*/ */
int px4_deamon_app_main(int argc, char *argv[]) int px4_daemon_app_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1)
usage("missing command"); usage("missing command");
@ -85,17 +89,17 @@ int px4_deamon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
printf("deamon already running\n"); warnx("daemon already running\n");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
} }
thread_should_exit = false; thread_should_exit = false;
deamon_task = task_spawn("deamon", daemon_task = task_spawn("daemon",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
4096, 4096,
px4_deamon_thread_main, px4_daemon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0); exit(0);
} }
@ -107,9 +111,9 @@ int px4_deamon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
printf("\tdeamon app is running\n"); warnx("\trunning\n");
} else { } else {
printf("\tdeamon app not started\n"); warnx("\tnot started\n");
} }
exit(0); exit(0);
} }
@ -118,18 +122,18 @@ int px4_deamon_app_main(int argc, char *argv[])
exit(1); exit(1);
} }
int px4_deamon_thread_main(int argc, char *argv[]) { int px4_daemon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n"); warnx("[daemon] starting\n");
thread_running = true; thread_running = true;
while (!thread_should_exit) { while (!thread_should_exit) {
printf("Hello Deamon!\n"); warnx("Hello daemon!\n");
sleep(10); sleep(10);
} }
printf("[deamon] exiting.\n"); warnx("[daemon] exiting.\n");
thread_running = false; thread_running = false;

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

@ -683,7 +683,8 @@ int KalmanNav::correctPos()
// fault detetcion // fault detetcion
float beta = y.dot(S.inverse() * y); float beta = y.dot(S.inverse() * y);
if (beta > _faultPos.get()) { static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
printf("fault in gps: beta = %8.4f\n", (double)beta); printf("fault in gps: beta = %8.4f\n", (double)beta);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(y(0) / sqrtf(RPos(0, 0))), double(y(0) / sqrtf(RPos(0, 0))),
@ -693,6 +694,7 @@ int KalmanNav::correctPos()
double(y(4) / sqrtf(RPos(4, 4))), double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5)))); double(y(5) / sqrtf(RPos(5, 5))));
} }
counter++;
return ret_ok; return ret_ok;
} }

15
src/modules/att_pos_estimator_ekf/kalman_main.cpp
View File

@ -44,6 +44,7 @@
#include <string.h> #include <string.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <math.h> #include <math.h>
#include "KalmanNav.hpp" #include "KalmanNav.hpp"
@ -73,7 +74,7 @@ usage(const char *reason)
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n"); warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
exit(1); exit(1);
} }
@ -94,13 +95,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
printf("kalman_demo already running\n"); warnx("already running");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
} }
thread_should_exit = false; thread_should_exit = false;
deamon_task = task_spawn("kalman_demo", deamon_task = task_spawn("att_pos_estimator_ekf",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
4096, 4096,
@ -116,10 +117,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
printf("\tkalman_demo app is running\n"); warnx("is running\n");
} else { } else {
printf("\tkalman_demo app not started\n"); warnx("not started\n");
} }
exit(0); exit(0);
@ -132,7 +133,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
int kalman_demo_thread_main(int argc, char *argv[]) int kalman_demo_thread_main(int argc, char *argv[])
{ {
printf("[kalman_demo] starting\n"); warnx("starting\n");
using namespace math; using namespace math;
@ -144,7 +145,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
nav.update(); nav.update();
} }
printf("[kalman_demo] exiting.\n"); printf("exiting.\n");
thread_running = false; thread_running = false;

5
src/modules/attitude_estimator_so3_comp/README Normal file
View File

@ -0,0 +1,5 @@
Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
Option -d is for debugging packet. See code for detailed packet structure.

833
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp Executable file
View File

@ -0,0 +1,833 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_main.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include <poll.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#ifdef __cplusplus
extern "C" {
#endif
#include "attitude_estimator_so3_comp_params.h"
#ifdef __cplusplus
}
#endif
extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
static bool bFilterInit = false;
//! Auxiliary variables to reduce number of repeated operations
static float q0q0, q0q1, q0q2, q0q3;
static float q1q1, q1q2, q1q3;
static float q2q2, q2q3;
static float q3q3;
//! Serial packet related
static int uart;
static int baudrate;
/**
* Mainloop of attitude_estimator_so3_comp.
*/
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
exit(1);
}
/**
* The attitude_estimator_so3_comp app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int attitude_estimator_so3_comp_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("attitude_estimator_so3_comp already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
attitude_estimator_so3_comp_task = task_spawn("attitude_estimator_so3_comp",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12400,
attitude_estimator_so3_comp_thread_main,
(const char **)argv);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
while(thread_running){
usleep(200000);
printf(".");
}
printf("terminated.");
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tattitude_estimator_so3_comp app is running\n");
} else {
printf("\tattitude_estimator_so3_comp app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float number) {
volatile long i;
volatile float x, y;
volatile const float f = 1.5F;
x = number * 0.5F;
y = number;
i = * (( long * ) &y);
i = 0x5f375a86 - ( i >> 1 );
y = * (( float * ) &i);
y = y * ( f - ( x * y * y ) );
return y;
}
//! Using accelerometer, sense the gravity vector.
//! Using magnetometer, sense yaw.
void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay, -az);
initialPitch = atan2(ax, -az);
cosRoll = cosf(initialRoll);
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
// auxillary variables to reduce number of repeated operations, for 1st pass
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
float recipNorm;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
//! Make filter converge to initial solution faster
//! This function assumes you are in static position.
//! WARNING : in case air reboot, this can cause problem. But this is very
//! unlikely happen.
if(bFilterInit == false)
{
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = true;
}
//! If magnetometer measurement is available, use it.
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
float hx, hy, hz, bx, bz;
float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement
// Will sqrt work better? PX4 system is powerful enough?
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy);
bz = hz;
// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx);
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
float halfvx, halfvy, halfvz;
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += ay * halfvz - az * halfvy;
halfey += az * halfvx - ax * halfvz;
halfez += ax * halfvy - ay * halfvx;
}
// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt;
gx += gyro_bias[0]; // apply integral feedback
gy += gyro_bias[1];
gz += gyro_bias[2];
}
else {
gyro_bias[0] = 0.0f; // prevent integral windup
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
//! Integrate rate of change of quaternion
#if 0
gx *= (0.5f * dt); // pre-multiply common factors
gy *= (0.5f * dt);
gz *= (0.5f * dt);
#endif
// Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
//! q_k = q_{k-1} + dt*\dot{q}
//! \dot{q} = 0.5*q \otimes P(\omega)
dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
q0 += dt*dq0;
q1 += dt*dq1;
q2 += dt*dq2;
q3 += dt*dq3;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
void send_uart_byte(char c)
{
write(uart,&c,1);
}
void send_uart_bytes(uint8_t *data, int length)
{
write(uart,data,(size_t)(sizeof(uint8_t)*length));
}
void send_uart_float(float f) {
uint8_t * b = (uint8_t *) &f;
//! Assume float is 4-bytes
for(int i=0; i<4; i++) {
uint8_t b1 = (b[i] >> 4) & 0x0f;
uint8_t b2 = (b[i] & 0x0f);
uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
send_uart_bytes(&c1,1);
send_uart_bytes(&c2,1);
}
}
void send_uart_float_arr(float *arr, int length)
{
for(int i=0;i<length;++i)
{
send_uart_float(arr[i]);
send_uart_byte(',');
}
}
int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL;
}
printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
*is_usb = false;
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
} else {
*is_usb = true;
}
return uart;
}
/*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
*/
/*
* EKF Attitude Estimator main function.
*
* Estimates the attitude recursively once started.
*
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Serial debug related
int ch;
struct termios uart_config_original;
bool usb_uart;
bool debug_mode = false;
char *device_name = "/dev/ttyS2";
baudrate = 115200;
//! Time constant
float dt = 0.005f;
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f};
float mag[3] = {0.0f, 0.0f, 0.0f};
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
//! -d <device_name>, default : /dev/ttyS2
//! -b <baud_rate>, default : 115200
while ((ch = getopt(argc,argv,"d:b:")) != EOF){
switch(ch){
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if(baudrate == 0)
printf("invalid baud rate '%s'",optarg);
break;
case 'd':
device_name = optarg;
debug_mode = true;
break;
default:
usage("invalid argument");
}
}
if(debug_mode){
printf("Opening debugging port for 3D visualization\n");
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
printf("could not open %s", device_name);
else
printf("Open port success\n");
}
// print text
printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
//! Initialize attitude vehicle uORB message.
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 4);
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_comp_params so3_comp_params;
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
/* initialize parameter handles */
parameters_init(&so3_comp_param_handles);
uint64_t start_time = hrt_absolute_time();
bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* register the perf counter */
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
/* Main loop*/
while (!thread_should_exit) {
struct pollfd fds[2];
fds[0].fd = sub_raw;
fds[0].events = POLLIN;
fds[1].fd = sub_params;
fds[1].events = POLLIN;
int ret = poll(fds, 2, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
if (!state.flag_hil_enabled) {
fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
}
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update);
/* update parameters */
parameters_update(&so3_comp_param_handles, &so3_comp_params);
}
/* only run filter if sensor values changed */
if (fds[0].revents & POLLIN) {
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
if (!initialized) {
gyro_offsets[0] += raw.gyro_rad_s[0];
gyro_offsets[1] += raw.gyro_rad_s[1];
gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
}
} else {
perf_begin(so3_comp_loop_perf);
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
acc[0] = raw.accelerometer_m_s2[0];
acc[1] = raw.accelerometer_m_s2[1];
acc[2] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
mag[0] = raw.magnetometer_ga[0];
mag[1] = raw.magnetometer_ga[1];
mag[2] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
dt = 0.005f;
parameters_update(&so3_comp_param_handles, &so3_comp_params);
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
uint64_t timing_start = hrt_absolute_time();
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
// Convert q->R.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation.
//Equation (290)
//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
/* Do something */
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
//! Euler angle rate. But it needs to be investigated again.
/*
att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3);
*/
att.rollspeed = gyro[0];
att.pitchspeed = gyro[1];
att.yawspeed = gyro[2];
att.rollacc = 0;
att.pitchacc = 0;
att.yawacc = 0;
//! Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
/* TODO: Bias estimation required */
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
}
perf_end(so3_comp_loop_perf);
//! This will print out debug packet to visualization software
if(debug_mode)
{
float quat[4];
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
send_uart_float_arr(quat,4);
send_uart_byte('\n');
}
}
}
}
loopcounter++;
}// while
thread_running = false;
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
return 0;
}

63
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c Executable file
View File

@ -0,0 +1,63 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include "attitude_estimator_so3_comp_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}

44
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h Executable file
View File

@ -0,0 +1,44 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.h
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_comp_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_comp_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);

8
src/modules/attitude_estimator_so3_comp/module.mk Normal file
View File

@ -0,0 +1,8 @@
#
# Attitude estimator (Nonlinear SO3 complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3_comp
SRCS = attitude_estimator_so3_comp_main.cpp \
attitude_estimator_so3_comp_params.c

17
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
View File

@ -47,6 +47,7 @@
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <controllib/fixedwing.hpp> #include <controllib/fixedwing.hpp>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <math.h> #include <math.h>
@ -80,7 +81,7 @@ usage(const char *reason)
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n"); fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\n\n");
exit(1); exit(1);
} }
@ -101,13 +102,13 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
printf("control_demo already running\n"); warnx("already running");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
} }
thread_should_exit = false; thread_should_exit = false;
deamon_task = task_spawn("control_demo", deamon_task = task_spawn("fixedwing_backside",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10, SCHED_PRIORITY_MAX - 10,
5120, 5120,
@ -128,10 +129,10 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
printf("\tcontrol_demo app is running\n"); warnx("is running");
} else { } else {
printf("\tcontrol_demo app not started\n"); warnx("not started");
} }
exit(0); exit(0);
@ -144,7 +145,7 @@ int fixedwing_backside_main(int argc, char *argv[])
int control_demo_thread_main(int argc, char *argv[]) int control_demo_thread_main(int argc, char *argv[])
{ {
printf("[control_Demo] starting\n"); warnx("starting");
using namespace control; using namespace control;
@ -156,7 +157,7 @@ int control_demo_thread_main(int argc, char *argv[])
autopilot.update(); autopilot.update();
} }
printf("[control_demo] exiting.\n"); warnx("exiting.");
thread_running = false; thread_running = false;
@ -165,6 +166,6 @@ int control_demo_thread_main(int argc, char *argv[])
void test() void test()
{ {
printf("beginning control lib test\n"); warnx("beginning control lib test");
control::basicBlocksTest(); control::basicBlocksTest();
} }

38
src/modules/gpio_led/gpio_led.c
View File

@ -48,6 +48,7 @@
#include <nuttx/wqueue.h> #include <nuttx/wqueue.h>
#include <nuttx/clock.h> #include <nuttx/clock.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <poll.h> #include <poll.h>
@ -64,6 +65,7 @@ struct gpio_led_s {
}; };
static struct gpio_led_s gpio_led_data; static struct gpio_led_s gpio_led_data;
static bool gpio_led_started = false;
__EXPORT int gpio_led_main(int argc, char *argv[]); __EXPORT int gpio_led_main(int argc, char *argv[]);
@ -75,7 +77,15 @@ int gpio_led_main(int argc, char *argv[])
{ {
int pin = GPIO_EXT_1; int pin = GPIO_EXT_1;
if (argc > 1) { if (argc < 2) {
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
} else {
/* START COMMAND HANDLING */
if (!strcmp(argv[1], "start")) {
if (argc > 2) {
if (!strcmp(argv[1], "-p")) { if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) { if (!strcmp(argv[2], "1")) {
pin = GPIO_EXT_1; pin = GPIO_EXT_1;
@ -84,7 +94,7 @@ int gpio_led_main(int argc, char *argv[])
pin = GPIO_EXT_2; pin = GPIO_EXT_2;
} else { } else {
printf("[gpio_led] Unsupported pin: %s\n", argv[2]); warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1); exit(1);
} }
} }
@ -95,11 +105,26 @@ int gpio_led_main(int argc, char *argv[])
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) { if (ret != 0) {
printf("[gpio_led] Failed to queue work: %d\n", ret); warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1); exit(1);
} else {
gpio_led_started = true;
} }
exit(0); exit(0);
/* STOP COMMAND HANDLING */
} else if (!strcmp(argv[1], "stop")) {
gpio_led_started = false;
/* INVALID COMMAND */
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
}
}
} }
void gpio_led_start(FAR void *arg) void gpio_led_start(FAR void *arg)
@ -110,7 +135,7 @@ void gpio_led_start(FAR void *arg)
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
if (priv->gpio_fd < 0) { if (priv->gpio_fd < 0) {
printf("[gpio_led] GPIO: open fail\n"); warnx("[gpio_led] GPIO: open fail\n");
return; return;
} }
@ -125,11 +150,11 @@ void gpio_led_start(FAR void *arg)
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) { if (ret != 0) {
printf("[gpio_led] Failed to queue work: %d\n", ret); warnx("[gpio_led] Failed to queue work: %d\n", ret);
return; return;
} }
printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
} }
void gpio_led_cycle(FAR void *arg) void gpio_led_cycle(FAR void *arg)
@ -187,5 +212,6 @@ void gpio_led_cycle(FAR void *arg)
priv->counter = 0; priv->counter = 0;
/* repeat cycle at 5 Hz*/ /* repeat cycle at 5 Hz*/
if (gpio_led_started)
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
} }

76
src/modules/px4iofirmware/mixer.cpp
View File

@ -85,6 +85,9 @@ static int mixer_callback(uintptr_t handle,
static MixerGroup mixer_group(mixer_callback, 0); static MixerGroup mixer_group(mixer_callback, 0);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
static void mixer_set_failsafe();
void void
mixer_tick(void) mixer_tick(void)
{ {
@ -102,12 +105,15 @@ mixer_tick(void)
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
} }
/* default to failsafe mixing */
source = MIX_FAILSAFE; source = MIX_FAILSAFE;
/* /*
* Decide which set of controls we're using. * Decide which set of controls we're using.
*/ */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* don't actually mix anything - we already have raw PWM values or /* don't actually mix anything - we already have raw PWM values or
@ -117,6 +123,7 @@ mixer_tick(void)
} else { } else {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* mix from FMU controls */ /* mix from FMU controls */
@ -132,15 +139,29 @@ mixer_tick(void)
} }
} }
/*
* Set failsafe status flag depending on mixing source
*/
if (source == MIX_FAILSAFE) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
/* /*
* Run the mixers. * Run the mixers.
*/ */
if (source == MIX_FAILSAFE) { if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */ /* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i]; r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) { } else if (source != MIX_NONE) {
float outputs[IO_SERVO_COUNT]; float outputs[IO_SERVO_COUNT];
@ -156,7 +177,7 @@ mixer_tick(void)
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */ /* scale to servo output */
r_page_servos[i] = (outputs[i] * 500.0f) + 1500; r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
} }
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
@ -175,7 +196,7 @@ mixer_tick(void)
bool should_arm = ( bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
/* FMU is available or FMU is not available but override is an option */ /* FMU is available or FMU is not available but override is an option */
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
@ -225,7 +246,7 @@ mixer_callback(uintptr_t handle,
case MIX_FAILSAFE: case MIX_FAILSAFE:
case MIX_NONE: case MIX_NONE:
/* XXX we could allow for configuration of per-output failsafe values */ control = 0.0f;
return -1; return -1;
} }
@ -273,8 +294,7 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND: case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length); isr_debug(2, "append %d", length);
/* check for overflow - this is really fatal */ /* check for overflow - this would be really fatal */
/* XXX could add just what will fit & try to parse, then repeat... */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return; return;
@ -293,8 +313,13 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */ /* if anything was parsed */
if (resid != mixer_text_length) { if (resid != mixer_text_length) {
/* ideally, this should test resid == 0 ? */ /* only set mixer ok if no residual is left over */
if (resid == 0) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else {
/* not yet reached the end of the mixer, set as not ok */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
}
isr_debug(2, "used %u", mixer_text_length - resid); isr_debug(2, "used %u", mixer_text_length - resid);
@ -303,8 +328,43 @@ mixer_handle_text(const void *buffer, size_t length)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid; mixer_text_length = resid;
/* update failsafe values */
mixer_set_failsafe();
} }
break; break;
} }
} }
static void
mixer_set_failsafe()
{
/*
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
/* set failsafe defaults to the values for all inputs = 0 */
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* scale to servo output */
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
}
/* disable the rest of the outputs */
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servo_failsafe[i] = 0;
}

4
src/modules/px4iofirmware/protocol.h
View File

@ -85,7 +85,7 @@
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
/* dynamic status page */ /* dynamic status page */
#define PX4IO_PAGE_STATUS 1 #define PX4IO_PAGE_STATUS 1
@ -104,6 +104,7 @@
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@ -148,6 +149,7 @@
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */

10
src/modules/px4iofirmware/registers.c
View File

@ -41,6 +41,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -178,8 +179,10 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
* PAGE 105 * PAGE 105
* *
* Failsafe servo PWM values * Failsafe servo PWM values
*
* Disable pulses as default.
*/ */
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
void void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@ -230,11 +233,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_FAILSAFE_PWM: case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */ /* XXX range-check value? */
r_page_servo_failsafe[offset] = *values; r_page_servo_failsafe[offset] = *values;
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
offset++; offset++;
num_values--; num_values--;
values++; values++;

134
src/modules/sdlog2/logbuffer.c Normal file
View File

@ -0,0 +1,134 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 logbuffer.c
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include "logbuffer.h"
int logbuffer_init(struct logbuffer_s *lb, int size)
{
lb->size = size;
lb->write_ptr = 0;
lb->read_ptr = 0;
lb->data = malloc(lb->size);
return (lb->data == 0) ? ERROR : OK;
}
int logbuffer_count(struct logbuffer_s *lb)
{
int n = lb->write_ptr - lb->read_ptr;
if (n < 0) {
n += lb->size;
}
return n;
}
int logbuffer_is_empty(struct logbuffer_s *lb)
{
return lb->read_ptr == lb->write_ptr;
}
bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
{
// bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1;
if (available < 0)
available += lb->size;
if (size > available) {
// buffer overflow
return false;
}
char *c = (char *) ptr;
int n = lb->size - lb->write_ptr; // bytes to end of the buffer
if (n < size) {
// message goes over end of the buffer
memcpy(&(lb->data[lb->write_ptr]), c, n);
lb->write_ptr = 0;
} else {
n = 0;
}
// now: n = bytes already written
int p = size - n; // number of bytes to write
memcpy(&(lb->data[lb->write_ptr]), &(c[n]), p);
lb->write_ptr = (lb->write_ptr + p) % lb->size;
return true;
}
int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part)
{
// bytes available to read
int available = lb->write_ptr - lb->read_ptr;
if (available == 0) {
return 0; // buffer is empty
}
int n = 0;
if (available > 0) {
// read pointer is before write pointer, all available bytes can be read
n = available;
*is_part = false;
} else {
// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
n = lb->size - lb->read_ptr;
*is_part = true;
}
*ptr = &(lb->data[lb->read_ptr]);
return n;
}
void logbuffer_mark_read(struct logbuffer_s *lb, int n)
{
lb->read_ptr = (lb->read_ptr + n) % lb->size;
}

68
src/modules/sdlog2/logbuffer.h Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 logbuffer.h
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
#ifndef SDLOG2_RINGBUFFER_H_
#define SDLOG2_RINGBUFFER_H_
#include <stdbool.h>
struct logbuffer_s {
// pointers and size are in bytes
int write_ptr;
int read_ptr;
int size;
char *data;
};
int logbuffer_init(struct logbuffer_s *lb, int size);
int logbuffer_count(struct logbuffer_s *lb);
int logbuffer_is_empty(struct logbuffer_s *lb);
bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size);
int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part);
void logbuffer_mark_read(struct logbuffer_s *lb, int n);
#endif

43
src/modules/sdlog2/module.mk Normal file
View File

@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# 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.
#
############################################################################
#
# sdlog2 Application
#
MODULE_COMMAND = sdlog2
# The main thread only buffers to RAM, needs a high priority
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
SRCS = sdlog2.c \
logbuffer.c

1178
src/modules/sdlog2/sdlog2.c Normal file
View File

File diff suppressed because it is too large Load Diff

98
src/modules/sdlog2/sdlog2_format.h Normal file
View File

@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 sdlog2_format.h
*
* General log format structures and macro.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
/*
Format characters in the format string for binary log messages
b : int8_t
B : uint8_t
h : int16_t
H : uint16_t
i : int32_t
I : uint32_t
f : float
n : char[4]
N : char[16]
Z : char[64]
c : int16_t * 100
C : uint16_t * 100
e : int32_t * 100
E : uint32_t * 100
L : int32_t latitude/longitude
M : uint8_t flight mode
q : int64_t
Q : uint64_t
*/
#ifndef SDLOG2_FORMAT_H_
#define SDLOG2_FORMAT_H_
#define LOG_PACKET_HEADER_LEN 3
#define LOG_PACKET_HEADER uint8_t head1, head2, msg_type;
#define LOG_PACKET_HEADER_INIT(id) .head1 = HEAD_BYTE1, .head2 = HEAD_BYTE2, .msg_type = id
// once the logging code is all converted we will remove these from
// this header
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
struct log_format_s {
uint8_t type;
uint8_t length; // full packet length including header
char name[4];
char format[16];
char labels[64];
};
#define LOG_FORMAT(_name, _format, _labels) { \
.type = LOG_##_name##_MSG, \
.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
}
#define LOG_FORMAT_MSG 0x80
#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
#endif /* SDLOG2_FORMAT_H_ */

191
src/modules/sdlog2/sdlog2_messages.h Normal file
View File

@ -0,0 +1,191 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 sdlog2_messages.h
*
* Log messages and structures definition.
*
* @author Anton Babushkin <rk3dov@gmail.com>
*/
#ifndef SDLOG2_MESSAGES_H_
#define SDLOG2_MESSAGES_H_
#include "sdlog2_format.h"
/* define message formats */
#pragma pack(push, 1)
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 1
struct log_TIME_s {
uint64_t t;
};
/* --- ATT - ATTITUDE --- */
#define LOG_ATT_MSG 2
struct log_ATT_s {
float roll;
float pitch;
float yaw;
};
/* --- ATSP - ATTITUDE SET POINT --- */
#define LOG_ATSP_MSG 3
struct log_ATSP_s {
float roll_sp;
float pitch_sp;
float yaw_sp;
};
/* --- IMU - IMU SENSORS --- */
#define LOG_IMU_MSG 4
struct log_IMU_s {
float acc_x;
float acc_y;
float acc_z;
float gyro_x;
float gyro_y;
float gyro_z;
float mag_x;
float mag_y;
float mag_z;
};
/* --- SENS - OTHER SENSORS --- */
#define LOG_SENS_MSG 5
struct log_SENS_s {
float baro_pres;
float baro_alt;
float baro_temp;
float diff_pres;
};
/* --- LPOS - LOCAL POSITION --- */
#define LOG_LPOS_MSG 6
struct log_LPOS_s {
float x;
float y;
float z;
float vx;
float vy;
float vz;
float hdg;
int32_t home_lat;
int32_t home_lon;
float home_alt;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
#define LOG_LPSP_MSG 7
struct log_LPSP_s {
float x;
float y;
float z;
float yaw;
};
/* --- GPS - GPS POSITION --- */
#define LOG_GPS_MSG 8
struct log_GPS_s {
uint64_t gps_time;
uint8_t fix_type;
float eph;
float epv;
int32_t lat;
int32_t lon;
float alt;
float vel_n;
float vel_e;
float vel_d;
float cog;
};
/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
#define LOG_ATTC_MSG 9
struct log_ATTC_s {
float roll;
float pitch;
float yaw;
float thrust;
};
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
unsigned char state;
unsigned char flight_mode;
unsigned char manual_control_mode;
unsigned char manual_sas_mode;
unsigned char armed;
float battery_voltage;
float battery_current;
float battery_remaining;
unsigned char battery_warning;
};
/* --- RC - RC INPUT CHANNELS --- */
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[8];
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
#define LOG_OUT0_MSG 12
struct log_OUT0_s {
float output[8];
};
#pragma pack(pop)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(ATT, "fff", "Roll,Pitch,Yaw"),
LOG_FORMAT(ATSP, "fff", "RollSP,PitchSP,YawSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
#endif /* SDLOG2_MESSAGES_H_ */

3
src/modules/systemlib/geo/geo.c
View File

@ -185,11 +185,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
double d_lat = lat_next_rad - lat_now_rad; double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad; double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
const double radius_earth = 6371000.0d; const double radius_earth = 6371000.0d;
return radius_earth * c; return radius_earth * c;
} }

16
src/modules/systemlib/geo/geo.h
View File

@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
*/ */
__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
/**
* Returns the distance to the next waypoint in meters.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
/**
* Returns the bearing to the next waypoint in radians.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);

4
src/systemcmds/mixer/mixer.c
View File

@ -88,8 +88,8 @@ load(const char *devname, const char *fname)
{ {
int dev; int dev;
FILE *fp; FILE *fp;
char line[80]; char line[120];
char buf[512]; char buf[2048];
/* open the device */ /* open the device */
if ((dev = open(devname, 0)) < 0) if ((dev = open(devname, 0)) < 0)

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

@ -185,12 +185,18 @@ pwm_main(int argc, char *argv[])
const char *arg = argv[0]; const char *arg = argv[0];
argv++; argv++;
if (!strcmp(arg, "arm")) { if (!strcmp(arg, "arm")) {
/* tell IO that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0); ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_ARM"); err(1, "PWM_SERVO_ARM");
continue; continue;
} }
if (!strcmp(arg, "disarm")) { if (!strcmp(arg, "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0); ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_DISARM"); err(1, "PWM_SERVO_DISARM");