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

Merge branch 'master' into fmuv2_bringup

This commit is contained in:
Lorenz Meier 2013-06-10 15:01:44 +02:00
parent 308ec6001a 42ce3112ad
commit 9444def5f8
47 changed files with 4318 additions and 477 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

59
Tools/logconv.py Normal file
View File

@ -0,0 +1,59 @@
#!/usr/bin/env python
"""Convert binary log generated by sdlog to CSV format
Usage: python logconv.py <log.bin>"""
__author__ = "Anton Babushkin"
__version__ = "0.1"
import struct, sys
def _unpack_packet(data):
s = ""
s += "Q" #.timestamp = buf.raw.timestamp,
s += "fff" #.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
s += "fff" #.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
s += "fff" #.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
s += "f" #.baro = buf.raw.baro_pres_mbar,
s += "f" #.baro_alt = buf.raw.baro_alt_meter,
s += "f" #.baro_temp = buf.raw.baro_temp_celcius,
s += "ffff" #.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
s += "ffffffff" #.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
s += "f" #.vbat = buf.batt.voltage_v,
s += "f" #.bat_current = buf.batt.current_a,
s += "f" #.bat_discharged = buf.batt.discharged_mah,
s += "ffff" #.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
s += "fff" #.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
s += "iii" #.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
s += "fff" #.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
s += "fffffffff" #.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
s += "fff" #.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
s += "ffff" #.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
s += "ffffff" #.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
s += "f" #.diff_pressure = buf.diff_pres.differential_pressure_pa,
s += "f" #.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
s += "f" #.true_airspeed = buf.airspeed.true_airspeed_m_s
s += "iii" # to align to 280
d = struct.unpack(s, data)
return d
def _main():
if len(sys.argv) < 2:
print "Usage:\npython logconv.py <log.bin>"
return
fn = sys.argv[1]
sysvector_size = 280
f = open(fn, "r")
while True:
data = f.read(sysvector_size)
if len(data) < sysvector_size:
break
a = []
for i in _unpack_packet(data):
a.append(str(i))
print ";".join(a)
f.close()
if __name__ == "__main__":
_main()

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()

5
makefiles/config_px4fmu_default.mk
View File

@ -57,12 +57,14 @@ MODULES += systemcmds/tests
MODULES += modules/commander
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF / other filters)
#
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/att_pos_estimator_ekf
@ -79,6 +81,7 @@ MODULES += modules/multirotor_pos_control
# Logging
#
MODULES += modules/sdlog
MODULES += modules/sdlog2
#
# Library modules

12
makefiles/firmware.mk
View File

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

13
makefiles/module.mk
View File

@ -183,11 +183,16 @@ CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibi
#
module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
##
## Object files we will generate from sources
##
#
# Object files we will generate from sources
#
OBJS = $(addsuffix .o,$(SRCS))
#
# Dependency files that will be auto-generated
#
DEPS = $(addsuffix .d,$(SRCS))
#
# SRCS -> OBJS rules
#
@ -219,3 +224,5 @@ $(MODULE_OBJ): $(OBJS) $(GLOBAL_DEPS)
clean:
$(Q) $(REMOVE) $(MODULE_PRELINK) $(OBJS)
-include $(DEPS)

6
makefiles/nuttx.mk
View File

@ -69,10 +69,14 @@ INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
LIB_DIRS += $(NUTTX_EXPORT_DIR)libs
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
LINK_DEPS += $(NUTTX_LIBS)
$(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE)
@$(ECHO) %% Unpacking $(NUTTX_ARCHIVE)
$(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE)
$(Q) $(TOUCH) $@
$(LDSCRIPT): $(NUTTX_CONFIG_HEADER)
$(NUTTX_LIBS): $(NUTTX_CONFIG_HEADER)

21
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
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
# Base CPU flags for each of the supported architectures.
#
@ -70,6 +70,14 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
-march=armv7-m \
-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.
#
ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
@ -91,8 +99,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
# enable precise stack overflow tracking
# note - requires corresponding support in NuttX
INSTRUMENTATIONDEFINES = -finstrument-functions \
-ffixed-r10
INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
@ -219,7 +227,7 @@ endef
define PRELINK
@$(ECHO) "PRELINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1
endef
# Update the archive $1 with the files in $2
@ -235,7 +243,7 @@ endef
define LINK
@$(ECHO) "LINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
$(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
endef
# Convert $1 from a linked object to a raw binary in $2
@ -280,6 +288,7 @@ define BIN_TO_OBJ
$(Q) $(OBJCOPY) $2 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end \
--rename-section .data=.rodata
$(Q) $(REMOVE) $2.c $2.c.o
endef

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

@ -243,7 +243,7 @@ endif
ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),GNU_EABI)
CROSSDEV ?= arm-none-eabi-
ARCROSSDEV ?= arm-none-eabi-
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
ifeq ($(CONFIG_ARCH_CORTEXM4),y)
ifeq ($(CONFIG_ARCH_FPU),y)
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
OBJDUMP = $(CROSSDEV)objdump
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
ARCHCPUFLAGS = -mcpu=cortex-m4 \
-mthumb \
-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_STANDARD_SERIAL=y
CONFIG_USART1_SERIAL_CONSOLE=y
CONFIG_USART1_SERIAL_CONSOLE=n
CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_UART4_SERIAL_CONSOLE=n
@ -561,7 +561,7 @@ CONFIG_START_MONTH=1
CONFIG_START_DAY=1
CONFIG_GREGORIAN_TIME=n
CONFIG_JULIAN_TIME=n
CONFIG_DEV_CONSOLE=y
CONFIG_DEV_CONSOLE=n
CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=y
@ -717,7 +717,7 @@ CONFIG_ARCH_BZERO=n
# zero for all dynamic allocations.
#
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=8
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=25
@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512
# Size of the serial receive/transmit buffers. Default 256.
#
CONFIG_CDCACM=y
CONFIG_CDCACM_CONSOLE=n
CONFIG_CDCACM_CONSOLE=y
#CONFIG_CDCACM_EP0MAXPACKET
CONFIG_CDCACM_EPINTIN=1
#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_STRERROR - Use strerror(errno)
# 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_DISABLESCRIPT - Disable scripting support
# CONFIG_NSH_DISABLEBG - Disable background commands
@ -988,7 +988,7 @@ CONFIG_NSH_BUILTIN_APPS=y
CONFIG_NSH_FILEIOSIZE=512
CONFIG_NSH_STRERROR=y
CONFIG_NSH_LINELEN=128
CONFIG_NSH_MAX_ARGUMENTS=12
CONFIG_NSH_MAXARGUMENTS=12
CONFIG_NSH_NESTDEPTH=8
CONFIG_NSH_DISABLESCRIPT=n
CONFIG_NSH_DISABLEBG=n

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

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

2
src/drivers/gps/ubx.cpp
View File

@ -739,7 +739,7 @@ UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
msg.msg_class = msg_class;
msg.msg_id = msg_id;
msg.rate = rate;
send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
}
void

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

@ -329,7 +329,7 @@ HMC5883::HMC5883(int bus) :
_calibrated(false)
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// default scaling
_scale.x_offset = 0;

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

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

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

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,30 +34,44 @@
/**
* @file messages.c
* @author Simon Wilks <sjwilks@gmail.com>
*
*/
#include "messages.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/geo/geo.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/home_position.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 gps_sub = -1;
static int home_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));
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));
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 */
struct sensor_combined_s raw;
@ -74,26 +88,144 @@ void build_eam_response(uint8_t *buffer, int *size)
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
msg.sensor_id = EAM_SENSOR_ID;
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_id = EAM_SENSOR_TEXT_ID;
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);
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
msg.altitude_L = (uint8_t)alt & 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);
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
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;
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);
}

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

@ -44,11 +44,6 @@
#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.
* 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.
@ -66,18 +61,18 @@
#define TEMP_ZERO_CELSIUS 0x14
/* Electric Air Module (EAM) constants. */
#define ELECTRIC_AIR_MODULE 0x8e
#define EAM_SENSOR_ID 0xe0
#define EAM_SENSOR_ID 0x8e
#define EAM_SENSOR_TEXT_ID 0xe0
/* The Electric Air Module message. */
struct eam_module_msg {
uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor ID */
uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor */
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_inverse2;
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
uint8_t cell2_L;
uint8_t cell3_L;
uint8_t cell4_L;
@ -95,30 +90,107 @@ struct eam_module_msg {
uint8_t batt1_voltage_H;
uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */
uint8_t batt2_voltage_H;
uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */
uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */
uint8_t temperature2;
uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */
uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */
uint8_t altitude_H;
uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */
uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */
uint8_t current_H;
uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */
uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */
uint8_t main_voltage_H;
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
uint8_t battery_capacity_H;
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */
uint8_t climbrate_H;
uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */
uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */
uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */
uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */
uint8_t rpm_H;
uint8_t electric_min; /**< Flight time in minutes. */
uint8_t electric_sec; /**< Flight time in seconds. */
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
uint8_t electric_min; /**< Flight time in minutes. */
uint8_t electric_sec; /**< Flight time in seconds. */
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
uint8_t speed_H;
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
uint8_t stop; /**< Stop byte */
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 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_ */

524
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -76,7 +76,6 @@
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
#include <systemlib/ppm_decode.h>
#define I2C_BUS_SPEED 400000
#define UPDATE_RATE 400
@ -114,6 +113,7 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init(unsigned motors);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
@ -177,9 +177,10 @@ private:
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int mk_servo_arm(bool status);
int mk_servo_set(unsigned int chan, float val);
int mk_servo_set_test(unsigned int chan, float val);
int mk_servo_set(unsigned int chan, short val);
int mk_servo_set_value(unsigned int chan, short val);
int mk_servo_test(unsigned int chan);
short scaling(float val, float inMin, float inMax, float outMin, float outMax);
};
@ -207,20 +208,20 @@ const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslat
const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0};
int addrTranslator[] = {0,0,0,0,0,0,0,0};
int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
struct MotorData_t
{
struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
};
MotorData_t Motor[MAX_MOTORS];
@ -314,7 +315,7 @@ MK::init(unsigned motors)
/* start the IO interface task */
_task = task_spawn("mkblctrl",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX -20,
SCHED_PRIORITY_MAX - 20,
2048,
(main_t)&MK::task_main_trampoline,
nullptr);
@ -346,27 +347,11 @@ MK::set_mode(Mode mode)
*/
switch (mode) {
case MODE_2PWM:
if(_num_outputs == 4) {
//debug("MODE_QUAD");
} else if(_num_outputs == 6) {
//debug("MODE_HEXA");
} else if(_num_outputs == 8) {
//debug("MODE_OCTO");
}
//up_pwm_servo_init(0x3);
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE; /* default output rate */
break;
case MODE_4PWM:
if(_num_outputs == 4) {
//debug("MODE_QUADRO");
} else if(_num_outputs == 6) {
//debug("MODE_HEXA");
} else if(_num_outputs == 8) {
//debug("MODE_OCTO");
}
//up_pwm_servo_init(0xf);
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE; /* default output rate */
break;
@ -412,45 +397,55 @@ MK::set_frametype(int frametype)
int
MK::set_motor_count(unsigned count)
{
if(count > 0) {
if (count > 0) {
_num_outputs = count;
if(_px4mode == MAPPING_MK) {
if(_frametype == FRAME_PLUS) {
if (_px4mode == MAPPING_MK) {
if (_frametype == FRAME_PLUS) {
fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
}
if(_num_outputs == 4) {
if(_frametype == FRAME_PLUS) {
if (_num_outputs == 4) {
if (_frametype == FRAME_PLUS) {
memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
}
} else if(_num_outputs == 6) {
if(_frametype == FRAME_PLUS) {
} else if (_num_outputs == 6) {
if (_frametype == FRAME_PLUS) {
memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
}
} else if(_num_outputs == 8) {
if(_frametype == FRAME_PLUS) {
} else if (_num_outputs == 8) {
if (_frametype == FRAME_PLUS) {
memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
} else if(_frametype == FRAME_X) {
} else if (_frametype == FRAME_X) {
memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
}
}
} else {
fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
}
if(_num_outputs == 4) {
if (_num_outputs == 4) {
fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
} else if(_num_outputs == 6) {
} else if (_num_outputs == 6) {
fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
} else if(_num_outputs == 8) {
} else if (_num_outputs == 8) {
fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
}
@ -469,16 +464,35 @@ MK::set_motor_test(bool motortest)
return OK;
}
short
MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
{
short retVal = 0;
retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin;
if (retVal < outMin) {
retVal = outMin;
} else if (retVal > outMax) {
retVal = outMax;
}
return retVal;
}
void
MK::task_main()
{
long update_rate_in_us = 0;
float tmpVal = 0;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/* force a reset of the update rate */
_current_update_rate = 0;
@ -492,6 +506,8 @@ MK::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
@ -499,21 +515,14 @@ MK::task_main()
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].events = POLLIN;
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
log("starting");
long update_rate_in_us = 0;
/* loop until killed */
while (!_task_should_exit) {
@ -528,6 +537,7 @@ MK::task_main()
update_rate_in_ms = 2;
_update_rate = 500;
}
/* reject slower than 50 Hz updates */
if (update_rate_in_ms > 20) {
update_rate_in_ms = 20;
@ -539,8 +549,8 @@ MK::task_main()
_current_update_rate = _update_rate;
}
/* sleep waiting for data, but no more than a second */
int ret = ::poll(&fds[0], 2, 1000);
/* sleep waiting for data max 100ms */
int ret = ::poll(&fds[0], 2, 100);
/* this would be bad... */
if (ret < 0) {
@ -553,7 +563,7 @@ MK::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
@ -565,53 +575,52 @@ MK::task_main()
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned int i = 0; i < _num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
//outputs.output[i] = 1500 + (600 * outputs.output[i]);
//outputs.output[i] = 127 + (127 * outputs.output[i]);
/* nothing to do here */
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
if(outputs.output[i] < -1.0f) {
if (outputs.output[i] < -1.0f) {
outputs.output[i] = -1.0f;
} else if(outputs.output[i] > 1.0f) {
} else if (outputs.output[i] > 1.0f) {
outputs.output[i] = 1.0f;
} else {
outputs.output[i] = -1.0f;
}
}
/* don't go under BLCTRL_MIN_VALUE */
if(outputs.output[i] < BLCTRL_MIN_VALUE) {
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
}
//_motortest = true;
/* output to BLCtrl's */
if(_motortest == true) {
mk_servo_test(i);
} else {
//mk_servo_set(i, outputs.output[i]);
mk_servo_set_test(i, outputs.output[i]); // 8Bit
}
/* output to BLCtrl's */
if (_motortest == true) {
mk_servo_test(i);
} else {
mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
}
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
/* how about an arming update? */
@ -622,29 +631,9 @@ MK::task_main()
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
////up_pwm_servo_arm(aa.armed && !aa.lockdown);
mk_servo_arm(aa.armed && !aa.lockdown);
}
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i=0; i<rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
}
@ -666,7 +655,7 @@ MK::task_main()
}
int
int
MK::mk_servo_arm(bool status)
{
_armed = status;
@ -680,12 +669,13 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
_retries = 50;
uint8_t foundMotorCount = 0;
for(unsigned i=0; i<MAX_MOTORS; i++) {
for (unsigned i = 0; i < MAX_MOTORS; i++) {
Motor[i].Version = 0;
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
Motor[i].State = 0;
Motor[i].ReadMode = 0;
Motor[i].RawPwmValue = 0;
Motor[i].Current = 0;
Motor[i].MaxPWM = 0;
Motor[i].Temperature = 0;
@ -695,34 +685,37 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
uint8_t msg = 0;
uint8_t result[3];
for(unsigned i=0; i< count; i++) {
for (unsigned i = 0; i < count; i++) {
result[0] = 0;
result[1] = 0;
result[2] = 0;
set_address( BLCTRL_BASE_ADDR + i );
set_address(BLCTRL_BASE_ADDR + i);
if (OK == transfer(&msg, 1, &result[0], 3)) {
Motor[i].Current = result[0];
Motor[i].MaxPWM = result[1];
Motor[i].Temperature = result[2];
Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
foundMotorCount++;
if(Motor[i].MaxPWM == 250) {
if (Motor[i].MaxPWM == 250) {
Motor[i].Version = BLCTRL_NEW;
} else {
Motor[i].Version = BLCTRL_OLD;
}
}
}
if(showOutput) {
fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
for(unsigned i=0; i< foundMotorCount; i++) {
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
if (showOutput) {
fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount);
for (unsigned i = 0; i < foundMotorCount; i++) {
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
}
if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
}
}
@ -734,122 +727,136 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
int
MK::mk_servo_set(unsigned int chan, float val)
MK::mk_servo_set(unsigned int chan, short val)
{
float tmpVal = 0;
short tmpVal = 0;
_retries = 0;
uint8_t result[3] = { 0,0,0 };
uint8_t msg[2] = { 0,0 };
uint8_t rod=0;
uint8_t result[3] = { 0, 0, 0 };
uint8_t msg[2] = { 0, 0 };
uint8_t rod = 0;
uint8_t bytesToSendBL2 = 2;
tmpVal = val;
tmpVal = (1023 + (1023 * val));
if(tmpVal > 2047) {
tmpVal = 2047;
if (tmpVal > 1024) {
tmpVal = 1024;
} else if (tmpVal < 0) {
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
Motor[chan].SetPoint = (uint8_t) tmpVal / 3; // divide 8
Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8; // rest of divide 8
//rod = (uint8_t) tmpVal % 8;
//Motor[chan].SetPointLowerBits = rod<<1; // rest of divide 8
Motor[chan].SetPointLowerBits = 0;
if(_armed == false) {
if (_armed == false) {
Motor[chan].SetPoint = 0;
Motor[chan].SetPointLowerBits = 0;
}
//if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
if (Motor[chan].Version == BLCTRL_OLD) {
/*
* Old BL-Ctrl 8Bit served. Version < 2.0
*/
msg[0] = Motor[chan].SetPoint;
if (Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], 1, &result[0], 2)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = 255;;
if(Motor[chan].Version == BLCTRL_OLD) {
/*
* Old BL-Ctrl 8Bit served. Version < 2.0
*/
msg[0] = Motor[chan].SetPoint;
if(Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], 1, &result[0], 2)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = 255;;
} else {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
if (OK != transfer(&msg[0], 1, nullptr, 0)) {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
/*
* New BL-Ctrl 11Bit served. Version >= 2.0
*/
msg[0] = Motor[chan].SetPoint;
msg[1] = Motor[chan].SetPointLowerBits;
if(Motor[chan].SetPointLowerBits == 0) {
bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time
if (OK != transfer(&msg[0], 1, nullptr, 0)) {
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
if(Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = result[2];
} else {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
}
}
Motor[chan].RoundCount++;
} else {
/*
* New BL-Ctrl 11Bit served. Version >= 2.0
*/
msg[0] = Motor[chan].SetPoint;
msg[1] = Motor[chan].SetPointLowerBits;
if (Motor[chan].SetPointLowerBits == 0) {
bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time
}
if (Motor[chan].RoundCount >= 16) {
// on each 16th cyle we read out the status messages from the blctrl
if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
Motor[chan].Current = result[0];
Motor[chan].MaxPWM = result[1];
Motor[chan].Temperature = result[2];
} else {
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
Motor[chan].RoundCount = 0;
} else {
if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
}
}
}
Motor[chan].RoundCount++;
//}
if(showDebug == true) {
if (showDebug == true) {
debugCounter++;
if(debugCounter == 2000) {
if (debugCounter == 2000) {
debugCounter = 0;
for(int i=0; i<_num_outputs; i++){
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
for (int i = 0; i < _num_outputs; i++) {
if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
}
}
fprintf(stderr, "\n");
}
}
return 0;
}
int
MK::mk_servo_set_test(unsigned int chan, float val)
MK::mk_servo_set_value(unsigned int chan, short val)
{
_retries = 0;
int ret;
short tmpVal = 0;
uint8_t msg[2] = { 0, 0 };
float tmpVal = 0;
tmpVal = val;
uint8_t msg[2] = { 0,0 };
if (tmpVal > 1024) {
tmpVal = 1024;
tmpVal = (1023 + (1023 * val));
if(tmpVal > 2048) {
tmpVal = 2048;
} else if (tmpVal < 0) {
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
if(_armed == false) {
if (_armed == false) {
Motor[chan].SetPoint = 0;
Motor[chan].SetPointLowerBits = 0;
}
@ -860,7 +867,6 @@ MK::mk_servo_set_test(unsigned int chan, float val)
ret = transfer(&msg[0], 1, nullptr, 0);
ret = OK;
return ret;
}
@ -868,59 +874,61 @@ MK::mk_servo_set_test(unsigned int chan, float val)
int
MK::mk_servo_test(unsigned int chan)
{
int ret=0;
int ret = 0;
float tmpVal = 0;
float val = -1;
_retries = 0;
uint8_t msg[2] = { 0,0 };
uint8_t msg[2] = { 0, 0 };
if(debugCounter >= MOTOR_SPINUP_COUNTER) {
if (debugCounter >= MOTOR_SPINUP_COUNTER) {
debugCounter = 0;
_motor++;
if(_motor < _num_outputs) {
if (_motor < _num_outputs) {
fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
}
if(_motor >= _num_outputs) {
if (_motor >= _num_outputs) {
_motor = -1;
_motortest = false;
}
}
debugCounter++;
if(_motor == chan) {
if (_motor == chan) {
val = BLCTRL_MIN_VALUE;
} else {
val = -1;
}
tmpVal = (1023 + (1023 * val));
if(tmpVal > 2048) {
tmpVal = 2048;
tmpVal = (511 + (511 * val));
if (tmpVal > 1024) {
tmpVal = 1024;
}
//Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
//Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
if(_motor != chan) {
if (_motor != chan) {
Motor[chan].SetPoint = 0;
Motor[chan].SetPointLowerBits = 0;
}
if(Motor[chan].Version == BLCTRL_OLD) {
if (Motor[chan].Version == BLCTRL_OLD) {
msg[0] = Motor[chan].SetPoint;
} else {
msg[0] = Motor[chan].SetPoint;
msg[1] = Motor[chan].SetPointLowerBits;
}
set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
if(Motor[chan].Version == BLCTRL_OLD) {
if (Motor[chan].Version == BLCTRL_OLD) {
ret = transfer(&msg[0], 1, nullptr, 0);
} else {
ret = transfer(&msg[0], 2, nullptr, 0);
}
@ -931,9 +939,9 @@ MK::mk_servo_test(unsigned int chan)
int
MK::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@ -947,7 +955,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
int ret;
// XXX disabled, confusing users
//debug("ioctl 0x%04x 0x%08x", cmd, arg);
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
@ -978,32 +985,37 @@ int
MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
////up_pwm_servo_arm(true);
mk_servo_arm(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
////up_pwm_servo_arm(false);
mk_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
ret = OK;
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
ret = OK;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
if (arg < 2150) {
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
/* fake an update to the selected 'servo' channel */
if ((arg >= 0) && (arg <= 255)) {
channel = cmd - PWM_SERVO_SET(0);
//mk_servo_set(channel, arg);
} else {
ret = -EINVAL;
}
@ -1012,20 +1024,20 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
/* copy the current output value from the channel */
*(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue;
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
//*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
/*
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
*/
*(unsigned *)arg = _num_outputs;
break;
case MIXERIOCRESET:
@ -1078,6 +1090,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
@ -1091,6 +1104,38 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
/*
this implements PWM output via a write() method, for compatibility
with px4io
*/
ssize_t
MK::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
// loeschen uint16_t values[4];
uint16_t values[8];
// loeschen if (count > 4) {
// loeschen // we only have 4 PWM outputs on the FMU
// loeschen count = 4;
// loeschen }
if (count > _num_outputs) {
// we only have 8 I2C outputs in the driver
count = _num_outputs;
}
// allow for misaligned values
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
Motor[i].RawPwmValue = (unsigned short)values[i];
mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
}
return count * 2;
}
void
MK::gpio_reset(void)
{
@ -1229,10 +1274,10 @@ enum MappingMode {
MAPPING_PX4,
};
enum FrameType {
FRAME_PLUS = 0,
FRAME_X,
};
enum FrameType {
FRAME_PLUS = 0,
FRAME_X,
};
PortMode g_port_mode;
@ -1297,18 +1342,17 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
g_mk->set_motor_test(motortest);
/* (re)set count of used motors */
////g_mk->set_motor_count(motorcount);
/* count used motors */
do {
if(g_mk->mk_check_for_blctrl(8, false) != 0) {
if (g_mk->mk_check_for_blctrl(8, false) != 0) {
shouldStop = 4;
} else {
shouldStop++;
}
sleep(1);
} while ( shouldStop < 3);
} while (shouldStop < 3);
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
@ -1375,7 +1419,8 @@ mkblctrl_main(int argc, char *argv[])
if (argc > i + 1) {
bus = atoi(argv[i + 1]);
newMode = true;
} else {
} else {
errx(1, "missing argument for i2c bus (-b)");
return 1;
}
@ -1384,17 +1429,21 @@ mkblctrl_main(int argc, char *argv[])
/* look for the optional frame parameter */
if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
if (argc > i + 1) {
if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
px4mode = MAPPING_MK;
newMode = true;
if(strcmp(argv[i + 1], "+") == 0) {
if (strcmp(argv[i + 1], "+") == 0) {
frametype = FRAME_PLUS;
} else {
frametype = FRAME_X;
}
} else {
errx(1, "only + or x for frametype supported !");
}
} else {
errx(1, "missing argument for mkmode (-mkmode)");
return 1;
@ -1409,12 +1458,12 @@ mkblctrl_main(int argc, char *argv[])
/* look for the optional -h --help parameter */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
showHelp == true;
showHelp = true;
}
}
if(showHelp) {
if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
exit(1);
@ -1424,6 +1473,7 @@ mkblctrl_main(int argc, char *argv[])
if (g_mk == nullptr) {
if (mk_start(bus, motorcount) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
} else {
newMode = true;
}

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

@ -106,7 +106,7 @@ public:
* @param rate The rate in Hz actuator outpus are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
int set_update_rate(int rate);
/**
* Set the battery current scaling and bias
@ -114,7 +114,15 @@ public:
* @param amp_per_volt
* @param amp_bias
*/
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
@ -326,11 +334,11 @@ PX4IO::PX4IO() :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0),
_primary_pwm_device(false)
_battery_last_timestamp(0)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -689,6 +697,19 @@ PX4IO::io_set_control_state()
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
PX4IO::io_set_arming_state()
{
@ -1250,7 +1271,7 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
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 & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
((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_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((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);
printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
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_IBATT),
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_bias,
(double)_battery_mamphour_total);
@ -1474,7 +1496,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
ret = mixer_send(buf, strnlen(buf, 1024));
ret = mixer_send(buf, strnlen(buf, 2048));
break;
}
@ -1615,6 +1637,13 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0))
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 (;;) {
/* sweep all servos between 1000..2000 */
@ -1649,6 +1678,16 @@ test(void)
if (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);
}
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 (g_dev != nullptr) {
@ -1845,5 +1919,5 @@ px4io_main(int argc, char *argv[])
monitor();
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'");
}

2
src/drivers/stm32/drv_hrt.c
View File

@ -330,7 +330,7 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */
# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2500 /* shortest valid start gap */

130
src/examples/fixedwing_control/main.c
View File

@ -33,10 +33,13 @@
****************************************************************************/
/**
* @file main.c
* Implementation of a fixed wing attitude controller. This file is a complete
* fixed wing controller flying manual attitude control or auto waypoint control.
*
* Example implementation of a fixed wing attitude controller. This file is a complete
* fixed wing controller for manual attitude control or auto waypoint control.
* There is no need to touch any other system components to extend / modify the
* complete control architecture.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@ -60,7 +63,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
@ -73,8 +75,15 @@
#include "params.h"
/* Prototypes */
/**
* Daemon management function.
*
* This function allows to start / stop the background task (daemon).
* The purpose of it is to be able to start the controller on the
* command line, query its status and stop it, without giving up
* the command line to one particular process or the need for bg/fg
* ^Z support by the shell.
*/
__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
@ -88,10 +97,34 @@ int fixedwing_control_thread_main(int argc, char *argv[]);
*/
static void usage(const char *reason);
/**
* Control roll and pitch angle.
*
* This very simple roll and pitch controller takes the current roll angle
* of the system and compares it to a reference. Pitch is controlled to zero and yaw remains
* uncontrolled (tutorial code, not intended for flight).
*
* @param att_sp The current attitude setpoint - the values the system would like to reach.
* @param att The current attitude. The controller should make the attitude match the setpoint
* @param speed_body The velocity of the system. Currently unused.
* @param rates_sp The angular rate setpoint. This is the output of the controller.
*/
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/**
* Control heading.
*
* This very simple heading to roll angle controller outputs the desired roll angle based on
* the current position of the system, the desired position (the setpoint) and the current
* heading.
*
* @param pos The current position of the system
* @param sp The current position setpoint
* @param att The current attitude
* @param att_sp The attitude setpoint. This is the output of the controller
*/
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
@ -103,7 +136,7 @@ static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
@ -148,13 +181,23 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
* Calculate heading error of current position to desired position
*/
/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
/*
* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
* so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
*/
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
/* calculate heading error */
float yaw_err = att->yaw - bearing;
/* apply control gain */
att_sp->roll_body = yaw_err * p.hdng_p;
float roll_command = yaw_err * p.hdng_p;
/* limit output, this commonly is a tuning parameter, too */
if (att_sp->roll_body < -0.6f) {
att_sp->roll_body = -0.6f;
} else if (att_sp->roll_body > 0.6f) {
att_sp->roll_body = 0.6f;
}
}
/* Main Thread */
@ -176,7 +219,32 @@ int fixedwing_control_thread_main(int argc, char *argv[])
parameters_init(&ph);
parameters_update(&ph, &p);
/* declare and safely initialize all structs to zero */
/*
* PX4 uses a publish/subscribe design pattern to enable
* multi-threaded communication.
*
* The most elegant aspect of this is that controllers and
* other processes can either 'react' to new data, or run
* at their own pace.
*
* PX4 developer guide:
* https://pixhawk.ethz.ch/px4/dev/shared_object_communication
*
* Wikipedia description:
* http://en.wikipedia.org/wiki/Publish–subscribe_pattern
*
*/
/*
* Declare and safely initialize all structs to zero.
*
* These structs contain the system state and things
* like attitude, position, the current waypoint, etc.
*/
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@ -192,20 +260,24 @@ int fixedwing_control_thread_main(int argc, char *argv[])
struct vehicle_global_position_setpoint_s global_sp;
memset(&global_sp, 0, sizeof(global_sp));
/* output structs */
/* output structs - this is what is sent to the mixer */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
/* publish actuator controls with zero values */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
/*
* Advertise that this controller will publish actuator
* control values and the rate setpoint
*/
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
/* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@ -215,8 +287,9 @@ int fixedwing_control_thread_main(int argc, char *argv[])
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
/* RC failsafe check */
bool throttle_half_once = false;
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }};
@ -235,7 +308,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, this will not really happen in practice */
/*
* Poll error, this will not really happen in practice,
* but its good design practice to make output an error message.
*/
warnx("poll error");
} else if (ret == 0) {
@ -261,6 +337,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_sp_sub, &global_sp_updated);
bool manual_sp_updated;
orb_check(manual_sp_sub, &manual_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@ -268,6 +346,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
/* currently speed in body frame is not used, but here for reference */
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
@ -285,15 +364,23 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
if (manual_sp_updated)
/* get the RC (or otherwise user based) input */
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.6f) &&
(manual_sp.throttle <= 1.0f)) {
throttle_half_once = true;
}
/* get the system status and the flight mode we're in */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* control */
/* if in auto mode, fly global position setpoint */
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
@ -305,7 +392,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
actuators.control[2] = 0.0f;
/* simple attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
@ -313,11 +400,12 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* set flaps to zero */
actuators.control[4] = 0.0f;
/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
if (vstatus.rc_signal_lost && throttle_half_once) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
@ -348,7 +436,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;

2
src/examples/fixedwing_control/params.c
View File

@ -45,7 +45,7 @@
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.1f);
/**
*

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

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

@ -683,7 +683,8 @@ int KalmanNav::correctPos()
// fault detetcion
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("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))),
@ -693,6 +694,7 @@ int KalmanNav::correctPos()
double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5))));
}
counter++;
return ret_ok;
}

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

@ -44,6 +44,7 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
@ -73,7 +74,7 @@ usage(const char *reason)
if (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);
}
@ -94,13 +95,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("kalman_demo already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("kalman_demo",
deamon_task = task_spawn("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4096,
@ -116,10 +117,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tkalman_demo app is running\n");
warnx("is running\n");
} else {
printf("\tkalman_demo app not started\n");
warnx("not started\n");
}
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[])
{
printf("[kalman_demo] starting\n");
warnx("starting\n");
using namespace math;
@ -144,7 +145,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
nav.update();
}
printf("[kalman_demo] exiting.\n");
printf("exiting.\n");
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 <controllib/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
@ -80,7 +81,7 @@ usage(const char *reason)
if (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);
}
@ -101,13 +102,13 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("control_demo already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("control_demo",
deamon_task = task_spawn("fixedwing_backside",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
@ -128,10 +129,10 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcontrol_demo app is running\n");
warnx("is running");
} else {
printf("\tcontrol_demo app not started\n");
warnx("not started");
}
exit(0);
@ -144,7 +145,7 @@ int fixedwing_backside_main(int argc, char *argv[])
int control_demo_thread_main(int argc, char *argv[])
{
printf("[control_Demo] starting\n");
warnx("starting");
using namespace control;
@ -156,7 +157,7 @@ int control_demo_thread_main(int argc, char *argv[])
autopilot.update();
}
printf("[control_demo] exiting.\n");
warnx("exiting.");
thread_running = false;
@ -165,6 +166,6 @@ int control_demo_thread_main(int argc, char *argv[])
void test()
{
printf("beginning control lib test\n");
warnx("beginning control lib test");
control::basicBlocksTest();
}

217
src/modules/gpio_led/gpio_led.c Normal file
View File

@ -0,0 +1,217 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.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 gpio_led.c
*
* Status LED via GPIO driver.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <stdbool.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
struct gpio_led_s {
struct work_s work;
int gpio_fd;
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
bool led_state;
int counter;
};
static struct gpio_led_s gpio_led_data;
static bool gpio_led_started = false;
__EXPORT int gpio_led_main(int argc, char *argv[]);
void gpio_led_start(FAR void *arg);
void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[])
{
int pin = GPIO_EXT_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[2], "1")) {
pin = GPIO_EXT_1;
} else if (!strcmp(argv[2], "2")) {
pin = GPIO_EXT_2;
} else {
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
}
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
} else {
gpio_led_started = true;
}
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)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* open GPIO device */
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
if (priv->gpio_fd < 0) {
warnx("[gpio_led] GPIO: open fail\n");
return;
}
/* configure GPIO pin */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* subscribe to vehicle status topic */
memset(&priv->status, 0, sizeof(priv->status));
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* add worker to queue */
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
return;
}
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
}
void gpio_led_cycle(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* check for status updates*/
bool status_updated;
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
/* select pattern for current status */
int pattern = 0;
if (priv->status.flag_system_armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x3f; // ****** solid (armed)
} else {
pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning)
}
} else {
if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm)
}
}
/* blink pattern */
bool led_state_new = (pattern & (1 << priv->counter)) != 0;
if (led_state_new != priv->led_state) {
priv->led_state = led_state_new;
if (led_state_new) {
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
} else {
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
}
priv->counter++;
if (priv->counter > 5)
priv->counter = 0;
/* repeat cycle at 5 Hz*/
if (gpio_led_started)
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
}

39
src/modules/gpio_led/module.mk Normal file
View File

@ -0,0 +1,39 @@
############################################################################
#
# 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.
#
############################################################################
#
# Status LED via GPIO driver
#
MODULE_COMMAND = gpio_led
SRCS = gpio_led.c

10
src/modules/mavlink/mavlink_receiver.c
View File

@ -429,11 +429,11 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil gps at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
// if ((timestamp - old_timestamp) > 10000000) {
// printf("receiving hil gps at %d hz\n", hil_frames/10);
// old_timestamp = timestamp;
// hil_frames = 0;
// }
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {

80
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);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
static void mixer_set_failsafe();
void
mixer_tick(void)
{
@ -102,13 +105,16 @@ mixer_tick(void)
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
}
/* default to failsafe mixing */
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* 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)) {
/* don't actually mix anything - we already have raw PWM values or
not a valid mixer. */
@ -117,6 +123,7 @@ mixer_tick(void)
} else {
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)) {
/* 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.
*/
if (source == MIX_FAILSAFE) {
/* 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];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[IO_SERVO_COUNT];
@ -156,7 +177,7 @@ mixer_tick(void)
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* 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++)
@ -175,7 +196,7 @@ mixer_tick(void)
bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_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) &&
/* 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) ))
@ -225,7 +246,7 @@ mixer_callback(uintptr_t handle,
case MIX_FAILSAFE:
case MIX_NONE:
/* XXX we could allow for configuration of per-output failsafe values */
control = 0.0f;
return -1;
}
@ -273,8 +294,7 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* check for overflow - this is really fatal */
/* XXX could add just what will fit & try to parse, then repeat... */
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
@ -293,8 +313,13 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
/* ideally, this should test resid == 0 ? */
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* only set mixer ok if no residual is left over */
if (resid == 0) {
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);
@ -303,8 +328,43 @@ mixer_handle_text(const void *buffer, size_t length)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
/* update failsafe values */
mixer_set_failsafe();
}
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

@ -111,7 +111,7 @@
#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_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 */
#define PX4IO_PAGE_STATUS 1
@ -130,6 +130,7 @@
#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_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_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@ -174,6 +175,7 @@
#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_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_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 <stdlib.h>
#include <string.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
*
* 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
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:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
offset++;
num_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_ */

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

@ -185,12 +185,11 @@ __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_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));
const double radius_earth = 6371000.0d;
return radius_earth * c;
return radius_earth * c;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)

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);
/**
* 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);
/**
* 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 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;
FILE *fp;
char line[80];
char buf[512];
char line[120];
char buf[2048];
/* open the device */
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];
argv++;
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);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
}
if (!strcmp(arg, "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");