fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete sdlog2

Browse Source
This commit is contained in:
Daniel Agar 2018-07-12 17:21:18 -04:00 committed by Lorenz Meier
parent 83fd5a5fd1
commit 8537863848
41 changed files with 20 additions and 5042 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -4,30 +4,24 @@
# Standard startup script for logging
#
if param compare SYS_LOGGER 0
set LOGGER_ARGS ""
if param compare SDLOG_MODE 1
then
sdlog2 start -r 100 -a -b 9 -t
else
set LOGGER_ARGS ""
if param compare SDLOG_MODE 1
then
set LOGGER_ARGS "-e"
fi
if param compare SDLOG_MODE 2
then
set LOGGER_ARGS "-f"
fi
if ver hwcmp AEROFC_V1
then
set LOGGER_ARGS "-m mavlink"
fi
logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS}
unset LOGGER_ARGS
set LOGGER_ARGS "-e"
fi
if param compare SDLOG_MODE 2
then
set LOGGER_ARGS "-f"
fi
if ver hwcmp AEROFC_V1
then
set LOGGER_ARGS "-m mavlink"
fi
logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS}
unset LOGGER_ARGS
unset LOGGER_BUF

1
Tools/astyle/files_to_check_code_style.sh
View File

@ -16,7 +16,6 @@ exec find src platforms \
-path src/lib/matrix -prune -o \
-path src/modules/commander -prune -o \
-path src/modules/micrortps_bridge/micro-CDR -prune -o \
-path src/modules/sdlog2 -prune -o \
-path src/modules/systemlib/uthash -prune -o \
-path src/modules/uavcan/libuavcan -prune -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

29
Tools/sdlog2/README.md
View File

@ -1,29 +0,0 @@
====== PX4 LOG CONVERSION ======
On each log session (commonly started and stopped by arming and disarming the vehicle) a new file logxxx.bin is created. In many cases there will be only one logfile named log001.bin (only one flight).
There are two conversion scripts in this ZIP file:
logconv.m: This is a MATLAB script which will automatically convert and display the flight data with a GUI. If running this script, the second script can be ignored.
sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run:
```
python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n ""
```
geo_tag_images.py: Use this script to geotag a set of images. It uses GPS time and file creation date to synchronize the images, so it needs that the images have a valid creation date. Can generate a KML file to view where the photos were taken in Google Earth (including height).
```sh
python geo_tag_images.py --logfile=mylog.bin --input=images/ --output=tagged/ --kml -v
python geo_tag_images.py -l=mylog.bin -i=images/ -o=tagged/ --kml -v
```
geotagging.py: Use this script to geotag a set of images. It uses the CAM trigger data from the log file for image association.
```sh
python geotagging.py --logfile=mylog.bin --input=images/ --output=tagged/
```
Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.

434
Tools/sdlog2/geo_tag_images.py
View File

@ -1,434 +0,0 @@
#!/usr/bin/env python
#
# Tag the images recorded during a flight with geo location extracted from
# a PX4 binary log file.
#
# This file accepts *.jpg format images and reads position information
# from a *.px4log file
#
# Example Syntax:
# python geo_tag_images.py --logfile=log001.px4log --input=images/ --output=imagesWithTag/ --offset=-0.4 -v
#
# Optional: Correct image times first
# jhead -exonly -ft -n%Y-%m-%d\ %H.%M.%S -ta+HH:MM:SS *.JPG
#
# Author: Hector Azpurua hector@azpurua.com
# Based on the script of Andreas Bircher
import os
import re
import sys
import bisect
import pyexiv2
import argparse
from lxml import etree
import datetime
import calendar
from shutil import copyfile
from subprocess import check_output
from pykml.factory import KML_ElementMaker as KML
from pykml.factory import GX_ElementMaker as GX
class GpsPosition(object):
def __init__(self, timestamp, lat, lon, alt):
self.timestamp = timestamp
self.lat = float(lat)
self.lon = float(lon)
self.alt = float(alt)
class Main:
def __init__(self):
"""
:param logfile:
:param input:
:param output:
:param offset:
:param verbose:
:return:
"""
args = self.get_arg()
self.logfile = args['logfile']
self.input = args['input']
self.output = args['output']
self.kml = args['kml']
self.verbose = args['verbose']
self.offset = args['offset']
self.time_thresh = args['threshold']
self.tdiff_list = []
self.non_processed_files = []
self.tagged_gps = []
print '[INFO] Loading logs and images locations...'
self.gps_list = self.load_gps_from_log(self.logfile, self.offset)
self.img_list = self.load_image_list(self.input)
if len(self.img_list) <= 0:
print '[ERROR] Cannot load JPG images from input folder, please check filename extensions.'
sys.exit(1)
if not os.path.exists(self.output):
os.makedirs(self.output)
if not self.output.endswith(os.path.sep):
self.output += os.path.sep
self.tag_images()
if self.kml and len(self.tdiff_list) > 0:
self.gen_kml()
if len(self.non_processed_files) > 0:
print '[WARNING] Some images werent processed (', len(self.non_processed_files), 'of', len(self.img_list), '):'
for elem in self.non_processed_files:
print '\t', elem
@staticmethod
def to_degree(value, loc):
"""
Convert a lat or lon value to degrees/minutes/seconds
:param value: the latitude or longitude value
:param loc: could be ["S", "N"] or ["W", "E"]
:return:
"""
if value < 0:
loc_value = loc[0]
elif value > 0:
loc_value = loc[1]
else:
loc_value = ""
absolute_value = abs(value)
deg = int(absolute_value)
t1 = (absolute_value - deg) * 60
minute = int(t1)
sec = round((t1 - minute) * 60, 5)
return deg, minute, sec, loc_value
@staticmethod
def gps_week_seconds_to_datetime(gpsweek, gpsmillis, leapmillis=0):
"""
Convert GPS week and seconds to datetime object, using leap milliseconds if necessary
:param gpsweek:
:param gpsmillis:
:param leapmillis:
:return:
"""
datetimeformat = "%Y-%m-%d %H:%M:%S.%f"
epoch = datetime.datetime.strptime(
"1980-01-06 00:00:00.000", datetimeformat)
elapsed = datetime.timedelta(
days=(gpsweek * 7), milliseconds=(gpsmillis + leapmillis))
return Main.utc_to_local(epoch + elapsed)
@staticmethod
def unix_microseconds_to_datetime(unix_us, offset=0):
"""
Convert unix microseconds to datetime object, using offset milliseconds if necessary
:param unix_us:
:param offset:
:return:
"""
# time in seconds
time_s = int(unix_us) / 1000000 + (offset / 1000)
datetime_from_unix = datetime.datetime.fromtimestamp(time_s)
return datetime_from_unix
@staticmethod
def utc_to_local(utc_dt):
"""
Convert UTC time in local time
:param utc_dt:
:return:
"""
# use integer timestamp to avoid precision loss
timestamp = calendar.timegm(utc_dt.timetuple())
local_dt = datetime.datetime.fromtimestamp(timestamp)
assert utc_dt.resolution >= datetime.timedelta(microseconds=1)
return local_dt.replace(microsecond=utc_dt.microsecond)
def gen_kml(self):
"""
Generate a KML file with keypoints on the locations of the pictures, including height
:return:
"""
style_dot = "sn_shaded_dot"
style_path = "red_path"
doc = KML.kml(
KML.Document(
KML.Name("GPS of the images"),
KML.Style(
KML.IconStyle(
KML.scale(0.4),
KML.Icon(
KML.href(
"http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png")
),
),
id=style_dot,
),
KML.Style(
KML.LineStyle(
KML.color('7f0000ff'),
KML.width(6),
GX.labelVisibility('1'),
),
id=style_path
)
)
)
# create points
for i, gps in enumerate(self.tagged_gps):
ii = i + 1
doc.Document.append(
KML.Placemark(
KML.styleUrl('#{0}'.format(style_dot)),
KML.Point(
KML.extrude(True),
KML.altitudeMode('absolute'),
KML.coordinates(
"{},{},{}".format(gps.lon, gps.lat, gps.alt))
),
KML.name(
str(ii)) if ii % 5 == 0 or ii == 1 else KML.name()
)
)
# create the path
doc.Document.append(
KML.Placemark(
KML.styleUrl('#{0}'.format(style_path)),
KML.LineString(
KML.altitudeMode('absolute'),
KML.coordinates(
' '.join(["{},{},{}".format(gps.lon, gps.lat, gps.alt)
for gps in self.tagged_gps])
)
)
)
)
s = etree.tostring(doc)
file_path = self.output + 'GoogleEarth_points.kml'
f = open(file_path, 'w')
f.write(s)
f.close()
print '[INFO] KML file generated on:', file_path
def get_closest_datetime_index(self, datetime_list, elem):
"""
Get the closest element between a list of datetime objects and a date
:param datetime_list:
:param elem:
:return:
"""
dlist_len = len(datetime_list)
i = bisect.bisect_left(datetime_list, elem)
# Cleanup of the indices
if i < 0:
i = 0
elif i >= dlist_len:
i = dlist_len - 1
date = datetime_list[i]
diff = abs((date - elem).total_seconds())
if diff > self.time_thresh:
return -1, diff
return i, diff
def set_gps_location(self, file_name, lat, lng, alt):
"""
Add the GPS tag and altitude to a image file
:param file_name:
:param lat:
:param lng:
:param alt:
:return:
"""
lat_deg = self.to_degree(lat, ["S", "N"])
lng_deg = self.to_degree(lng, ["W", "E"])
exiv_lat = (pyexiv2.Rational(lat_deg[0] * 60 + lat_deg[1], 60),
pyexiv2.Rational(lat_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
exiv_lng = (pyexiv2.Rational(lng_deg[0] * 60 + lng_deg[1], 60),
pyexiv2.Rational(lng_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
try:
exiv_image = pyexiv2.ImageMetadata(file_name)
exiv_image.read()
exiv_image["Exif.GPSInfo.GPSLatitude"] = exiv_lat
exiv_image["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3]
exiv_image["Exif.GPSInfo.GPSLongitude"] = exiv_lng
exiv_image["Exif.GPSInfo.GPSLongitudeRef"] = lng_deg[3]
exiv_image["Exif.GPSInfo.GPSAltitude"] = pyexiv2.Rational(alt, 1)
exiv_image["Exif.GPSInfo.GPSAltitudeRef"] = '0'
exiv_image["Exif.Image.GPSTag"] = 654
exiv_image["Exif.GPSInfo.GPSMapDatum"] = "WGS-84"
exiv_image["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0'
exiv_image.write(True)
except Exception as e:
print '[ERROR]', e
def load_gps_from_log(self, log_file, offset):
"""
Load gps list from PX4 binary log
:param log_file:
:param offset:
:return:
"""
gps_list = []
out = check_output(
["python", "sdlog2_dump.py", log_file, "-m GPS", "-v"])
for line in out.splitlines():
if not line.startswith("MSG GPS:"):
continue
vdict = {}
pairs = re.split(r'[;,:]\s*', line)
for pair in pairs:
e = pair.split('=')
if len(e) == 2:
vdict[e[0]] = float(e[1])
# PX4 GPS.GPSTime is unix time in microseconds
gps_time = vdict['GPSTime']
gps_lat = vdict['Lat']
gps_lon = vdict['Lon']
gps_alt = vdict['Alt']
date = self.unix_microseconds_to_datetime(gps_time, offset)
gps_list.append(GpsPosition(date, gps_lat, gps_lon, gps_alt))
return gps_list
def get_image_creation_date(self, filename):
exiv_image = pyexiv2.ImageMetadata(filename)
exiv_image.read()
# Prefer DateTime/Original over the other values
if 'Exif.Photo.DateTimeOriginal' in exiv_image:
cdate = exiv_image['Exif.Photo.DateTimeOriginal'].value
return cdate
elif 'Exif.Image.DateTime' in exiv_image:
cdate = exiv_image['Exif.Image.DateTime'].value
return cdate
else:
epoch = os.path.getmtime(filename)
return datetime.datetime.fromtimestamp(epoch)
def load_image_list(self, input_folder, file_type='jpg'):
"""
Load image list from a folder given a file type
:param input_folder:
:param file_type:
:return:
"""
self.img_list = [input_folder + filename for filename in os.listdir(input_folder)
if re.search(r'\.' + file_type + '$', filename, re.IGNORECASE)]
self.img_list = sorted(self.img_list)
return self.img_list
def tag_images(self):
"""
Tag the image list using the GPS loaded from the LOG file
:return:
"""
tagged_gps = []
img_size = len(self.img_list)
print '[INFO] Number of images:', img_size
print '[INFO] Number of gps logs:', len(self.gps_list)
dt_list = [x.timestamp for x in self.gps_list]
img_seq = 1
for i in xrange(img_size):
cdate = self.get_image_creation_date(self.img_list[i])
gps_i, img_tdiff = self.get_closest_datetime_index(dt_list, cdate)
base_path, filename = os.path.split(self.img_list[i])
if gps_i == -1:
self.non_processed_files.append(filename)
continue
closest_gps = self.gps_list[gps_i]
self.tdiff_list.append(img_tdiff)
if self.verbose:
msg = "[DEBUG] %s/%s) %s\n\timg %s -> gps %s (%ss)\n\tlat:%s, lon:%s, alt:%s".ljust(60) %\
(i + 1, img_size, filename, cdate, closest_gps.timestamp,
img_tdiff, closest_gps.lat, closest_gps.lon, closest_gps.alt)
print msg
output_filename = self.output + str(img_seq) + '_' + filename
copyfile(self.img_list[i], output_filename)
self.set_gps_location(
output_filename, closest_gps.lat, closest_gps.lon, closest_gps.alt)
self.tagged_gps.append(closest_gps)
img_seq += 1
if len(self.tdiff_list) > 0:
print '[INFO] Mean diff in seconds:', sum(self.tdiff_list) / float(len(self.tdiff_list))
@staticmethod
def get_arg():
parser = argparse.ArgumentParser(
description='Geotag script to add GPS info to pictures from PX4 binary log files.'
'It uses synchronized time to allocate GPS positions.'
)
parser.add_argument(
'-l', '--logfile', help='PX4 log file containing recorded positions.', required=True
)
parser.add_argument(
'-i', '--input', help='Input folder containing untagged images.', required=True
)
parser.add_argument(
'-o', '--output', help='Output folder to contain tagged images.', required=True
)
parser.add_argument(
'-t', '--threshold', help='Time threshold between the GPS time and the local image time.',
default=1, required=False, type=float
)
parser.add_argument(
'-of', '--offset', help='Time offset in MILLISECONDS between the GPS time and the local time.',
default=0, required=False, type=float
)
parser.add_argument(
'-kml', '--kml', help='Save the in KML format the information of all tagged images.',
required=False, action='store_true'
)
parser.add_argument(
'-v', '--verbose', help='Prints lots of information.',
required=False, action='store_true'
)
args = vars(parser.parse_args())
return args
if __name__ == "__main__":
m = Main()

237
Tools/sdlog2/geotagging.py
View File

@ -1,237 +0,0 @@
#!/usr/bin/env python
#
# __geotagging__
# Tag the images recorded during a flight with geo location extracted from
# a PX4 log file.
#
# This file accepts *.jpg format images and reads position information
# from a *.px4log file
#
# Example Syntax:
# python geotagging.py --logfile=log001.px4log --input=images/
# --output=imagesWithTag/
#
# Author: Andreas Bircher, Wingtra, http://wingtra.com, in 2016
#
import glob
import os
import pyexiv2
from shutil import copyfile
from optparse import OptionParser
import csv
from datetime import datetime, timedelta
class TriggerList(object):
def __init__(self):
self.CAMT_seq = []
self.CAMT_timestamp = []
self.GPOS_Lat = []
self.GPOS_Lon = []
self.GPOS_Alt = []
self.GPS_GPSTime = []
class ImageList(object):
def __init__(self):
self.jpg = []
self.raw = []
def to_degree(value, loc):
if value < 0:
loc_value = loc[0]
elif value > 0:
loc_value = loc[1]
else:
loc_value = ""
absolute_value = abs(value)
deg = int(absolute_value)
t1 = (absolute_value - deg) * 60
min = int(t1)
sec = round((t1 - min) * 60, 5)
return (deg, min, sec, loc_value)
def SetGpsLocation(file_name, gps_datetime, lat, lng, alt):
"""
Adding GPS tag
"""
lat_deg = to_degree(lat, ["S", "N"])
lng_deg = to_degree(lng, ["W", "E"])
exiv_lat = (pyexiv2.Rational(lat_deg[0] * 60 + lat_deg[1], 60), pyexiv2.Rational(
lat_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
exiv_lng = (pyexiv2.Rational(lng_deg[0] * 60 + lng_deg[1], 60), pyexiv2.Rational(
lng_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
exiv_image = pyexiv2.ImageMetadata(file_name)
exiv_image.read()
date_tag = exiv_image['Exif.Image.DateTime']
date_max = max(date_tag.value, gps_datetime)
date_min = min(date_tag.value, gps_datetime)
time_diff = date_max - date_min
if (time_diff > timedelta(seconds=5)):
print(
"WARNING, camera trigger and photo time different by {}".format(time_diff))
print(" Photo tag time: {}".format(date_tag.value))
print(" Camera trigger time: {}".format(gps_datetime))
exiv_image["Exif.GPSInfo.GPSLatitude"] = exiv_lat
exiv_image["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3]
exiv_image["Exif.GPSInfo.GPSLongitude"] = exiv_lng
exiv_image["Exif.GPSInfo.GPSLongitudeRef"] = lng_deg[3]
exiv_image["Exif.GPSInfo.GPSAltitude"] = pyexiv2.Rational(alt, 1)
exiv_image["Exif.GPSInfo.GPSAltitudeRef"] = '0'
exiv_image["Exif.Image.GPSTag"] = 654
exiv_image["Exif.GPSInfo.GPSMapDatum"] = "WGS-84"
exiv_image["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0'
exiv_image.write(True)
def LoadPX4log(px4_log_file):
"""
load px4 log file and extract trigger locations
"""
os.system('python sdlog2_dump.py ' + px4_log_file +
' -t time -m TIME -m CAMT -m GPOS -m GPS -f log.csv')
f = open('log.csv', 'rb')
reader = csv.reader(f)
headers = reader.next()
line = {}
for h in headers:
line[h] = []
for row in reader:
for h, v in zip(headers, row):
line[h].append(v)
trigger_list = TriggerList()
for seq in range(0, len(line['CAMT_seq']) - 1):
if line['CAMT_seq'][seq] != line['CAMT_seq'][seq + 1]:
trigger_list.CAMT_seq.append(line['CAMT_seq'][seq + 1])
trigger_list.CAMT_timestamp.append(line['CAMT_timestamp'][seq + 1])
trigger_list.GPOS_Lat.append(line['GPOS_Lat'][seq + 1])
trigger_list.GPOS_Lon.append(line['GPOS_Lon'][seq + 1])
trigger_list.GPOS_Alt.append(line['GPOS_Alt'][seq + 1])
trigger_list.GPS_GPSTime.append(line['GPS_GPSTime'][seq + 1])
return trigger_list
def LoadImageList(input_folder):
"""
load the image list
"""
image_list = ImageList()
for jpg_image in glob.glob(input_folder + "/*.jpg"):
image_list.jpg.append(jpg_image)
for jpg_image in glob.glob(input_folder + "/*.JPG"):
image_list.jpg.append(jpg_image)
for raw_image in glob.glob(input_folder + "/*.RC"):
image_list.raw.append(raw_image)
if len(image_list.jpg) != len(image_list.raw) and len(image_list.jpg) * len(image_list.raw) != 0:
print("Unequal number of jpg and raw images")
if len(image_list.jpg) == 0 and len(image_list.raw) == 0:
print("No images found")
image_list.jpg = sorted(image_list.jpg)
image_list.raw = sorted(image_list.raw)
return image_list
def FilterTrigger(trigger_list, image_list):
"""
filter triggers to allow exact matching with recorded images
"""
# filter trigger list to match the number of pics
if len(image_list.jpg) != len(trigger_list.CAMT_seq):
print('WARNING! differ number of jpg images ({}) and camera triggers ({})'.format(
len(image_list.jpg), len(trigger_list.CAMT_seq)))
n_overlap = min(len(image_list.jpg), len(trigger_list.CAMT_seq))
del image_list.jpg[n_overlap:]
if len(image_list.raw) != len(trigger_list.CAMT_seq):
print('WARNING! differ number of raw images ({}) and camera triggers ({})'.format(
len(image_list.raw), len(trigger_list.CAMT_seq)))
n_overlap = min(len(image_list.raw), len(trigger_list.CAMT_seq))
del image_list.raw[n_overlap:]
return trigger_list
def TagImages(trigger_list, image_list, output_folder):
"""
load px4 log file and extract trigger locations
"""
for image in range(len(image_list.jpg)):
print("############################################################")
print('Photo {}: {}'.format(image, image_list.jpg[image]))
cam_time = int(trigger_list.GPS_GPSTime[image]) / 1000000
gps_datetime = datetime.fromtimestamp(cam_time)
base_path, filename = os.path.split(image_list.jpg[image])
copyfile(image_list.jpg[image], output_folder + "/" + filename)
SetGpsLocation(output_folder + "/" + filename, gps_datetime, float(
trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]), float(trigger_list.GPOS_Alt[image]))
for image in range(len(image_list.raw)):
print("############################################################")
print('Photo {}: {}'.format(image, image_list.raw[image]))
cam_time = int(trigger_list.GPS_GPSTime[image]) / 1000000
gps_datetime = datetime.fromtimestamp(cam_time)
base_path, filename = os.path.split(image_list.raw[image])
copyfile(image_list.raw[image], output_folder + "/" + filename)
SetGpsLocation(output_folder + "/" + filename, gps_datetime, float(
trigger_list.GPOS_Lat[image]), float(trigger_list.GPOS_Lon[image]), float(trigger_list.GPOS_Alt[image]))
def main():
"""
Main method
"""
parser = OptionParser()
parser.add_option("-l", "--logfile", dest="LogFile",
help="PX4 log file containing recorded positions",
metavar="string")
parser.add_option("-i", "--input", dest="InputFolder",
help="Input folder containing untagged images in alphabetical order",
type="string")
parser.add_option("-o", "--output", dest="OutputFolder",
help="Output folder to contain tagged images",
type="string")
(options, args) = parser.parse_args()
if not options.LogFile:
print "please type python " \
"geotagging.py --logfile=[filename] --intput=[folder] [--output=[folder]]"
elif not options.InputFolder:
print "please type python " \
"geotagging.py --logfile=[filename] --intput=[folder] [--output=[folder]]s"
else:
trigger_list = LoadPX4log(options.LogFile)
image_list = LoadImageList(options.InputFolder)
if not options.OutputFolder:
options.OutputFolder = "imagesWithTag"
if not os.path.exists(options.OutputFolder):
os.makedirs(options.OutputFolder)
trigger_list = FilterTrigger(trigger_list, image_list)
TagImages(trigger_list, image_list, options.OutputFolder)
if __name__ == "__main__":
main()

535
Tools/sdlog2/logconv.m
View File

@ -1,535 +0,0 @@
% This Matlab Script can be used to import the binary logged values of the
% PX4FMU into data that can be plotted and analyzed.
%% ************************************************************************
% PX4LOG_PLOTSCRIPT: Main function
% ************************************************************************
function PX4Log_Plotscript
% Clear everything
clc
clear all
close all
% ************************************************************************
% SETTINGS
% ************************************************************************
% Set the path to your sysvector.bin file here
filePath = 'log001.bin';
% Set the minimum and maximum times to plot here [in seconds]
mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start]
maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end]
%Determine which data to plot. Not completely implemented yet.
bDisplayGPS=true;
%conversion factors
fconv_gpsalt=1; %[mm] to [m]
fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg]
fconv_timestamp=1E-6; % [microseconds] to [seconds]
% ************************************************************************
% Import the PX4 logs
% ************************************************************************
ImportPX4LogData();
%Translate min and max plot times to indices
time=double(sysvector.TIME_StartTime) .*fconv_timestamp;
mintime_log=time(1); %The minimum time/timestamp found in the log
maxtime_log=time(end); %The maximum time/timestamp found in the log
CurTime=mintime_log; %The current time at which to draw the aircraft position
[imintime,imaxtime]=FindMinMaxTimeIndices();
% ************************************************************************
% PLOT & GUI SETUP
% ************************************************************************
NrFigures=5;
NrAxes=10;
h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively
h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively
h.pathpoints=[]; % Temporary initiliazation of path points
% Setup the GUI to control the plots
InitControlGUI();
% Setup the plotting-GUI (figures, axes) itself.
InitPlotGUI();
% ************************************************************************
% DRAW EVERYTHING
% ************************************************************************
DrawRawData();
DrawCurrentAircraftState();
%% ************************************************************************
% *** END OF MAIN SCRIPT ***
% NESTED FUNCTION DEFINTIONS FROM HERE ON
% ************************************************************************
%% ************************************************************************
% IMPORTPX4LOGDATA (nested function)
% ************************************************************************
% Attention: This is the import routine for firmware from ca. 03/2013.
% Other firmware versions might require different import
% routines.
%% ************************************************************************
% IMPORTPX4LOGDATA (nested function)
% ************************************************************************
% Attention: This is the import routine for firmware from ca. 03/2013.
% Other firmware versions might require different import
% routines.
function ImportPX4LogData()
% ************************************************************************
% RETRIEVE SYSTEM VECTOR
% *************************************************************************
% //All measurements in NED frame
% Convert to CSV
%arg1 = 'log-fx61-20130721-2.bin';
arg1 = filePath;
delim = ',';
time_field = 'TIME';
data_file = 'data.csv';
csv_null = '';
if not(exist(data_file, 'file'))
s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) );
end
if exist(data_file, 'file')
%data = csvread(data_file);
sysvector = tdfread(data_file, ',');
% shot the flight time
time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1);
time_s = uint64(time_us*1e-6);
time_m = uint64(time_s/60);
time_s = time_s - time_m * 60;
disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]);
disp(['logfile conversion finished.' char(10)]);
else
disp(['file: ' data_file ' does not exist' char(10)]);
end
end
%% ************************************************************************
% INITCONTROLGUI (nested function)
% ************************************************************************
%Setup central control GUI components to control current time where data is shown
function InitControlGUI()
%**********************************************************************
% GUI size definitions
%**********************************************************************
dxy=5; %margins
%Panel: Plotctrl
dlabels=120;
dsliders=200;
dedits=80;
hslider=20;
hpanel1=40; %panel1
hpanel2=220;%panel2
hpanel3=3*hslider+4*dxy+3*dxy;%panel3.
width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width
height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height
%**********************************************************************
% Create GUI
%**********************************************************************
h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');
h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));
h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));
h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));
%%Control GUI-elements
%Slider: Current time
h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...
'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);
temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);
h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...
'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);
%Slider: MaxTime
h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...
'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...
'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
%Slider: MinTime
h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');
h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...
'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...
'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
%%Current data/state GUI-elements (Multiline-edit-box)
h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...
'HorizontalAlignment','left','parent',h.aircraftstatepanel);
h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...
'HorizontalAlignment','left','parent',h.guistatepanel);
end
%% ************************************************************************
% INITPLOTGUI (nested function)
% ************************************************************************
function InitPlotGUI()
% Setup handles to lines and text
h.markertext=[];
templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array
h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively
h.markerline(1:NrAxes)=0.0;
% Setup all other figures and axes for plotting
% PLOT WINDOW 1: GPS POSITION
h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');
h.axes(1)=axes();
set(h.axes(1),'Parent',h.figures(2));
% PLOT WINDOW 2: IMU, baro altitude
h.figures(3)=figure('Name', 'IMU / Baro Altitude');
h.axes(2)=subplot(4,1,1);
h.axes(3)=subplot(4,1,2);
h.axes(4)=subplot(4,1,3);
h.axes(5)=subplot(4,1,4);
set(h.axes(2:5),'Parent',h.figures(3));
% PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');
h.axes(6)=subplot(4,1,1);
h.axes(7)=subplot(4,1,2);
h.axes(8)=subplot(4,1,3);
h.axes(9)=subplot(4,1,4);
set(h.axes(6:9),'Parent',h.figures(4));
% PLOT WINDOW 4: LOG STATS
h.figures(5) = figure('Name', 'Log Statistics');
h.axes(10)=subplot(1,1,1);
set(h.axes(10:10),'Parent',h.figures(5));
end
%% ************************************************************************
% DRAWRAWDATA (nested function)
% ************************************************************************
%Draws the raw data from the sysvector, but does not add any
%marker-lines or so
function DrawRawData()
% ************************************************************************
% PLOT WINDOW 1: GPS POSITION & GUI
% ************************************************************************
figure(h.figures(2));
% Only plot GPS data if available
if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS)
%Draw data
plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ...
double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ...
double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.');
title(h.axes(1),'GPS Position Data(if available)');
xlabel(h.axes(1),'Latitude [deg]');
ylabel(h.axes(1),'Longitude [deg]');
zlabel(h.axes(1),'Altitude above MSL [m]');
grid on
%Reset path
h.pathpoints=0;
end
% ************************************************************************
% PLOT WINDOW 2: IMU, baro altitude
% ************************************************************************
figure(h.figures(3));
plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]);
title(h.axes(2),'Magnetometers [Gauss]');
legend(h.axes(2),'x','y','z');
plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]);
title(h.axes(3),'Accelerometers [m/s²]');
legend(h.axes(3),'x','y','z');
plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]);
title(h.axes(4),'Gyroscopes [rad/s]');
legend(h.axes(4),'x','y','z');
plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue');
if(bDisplayGPS)
hold on;
plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red');
hold off
legend('Barometric Altitude [m]','GPS Altitude [m]');
else
legend('Barometric Altitude [m]');
end
title(h.axes(5),'Altitude above MSL [m]');
% ************************************************************************
% PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
% ************************************************************************
figure(h.figures(4));
%Attitude Estimate
plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159);
title(h.axes(6),'Estimated attitude [deg]');
legend(h.axes(6),'roll','pitch','yaw');
%Actuator Controls
plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]);
title(h.axes(7),'Actuator control [-]');
legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]');
%Actuator Controls
plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]);
title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');
legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');
set(h.axes(8), 'YLim',[800 2200]);
%Airspeeds
plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime));
hold on
plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime));
hold off
%add GPS total airspeed here
title(h.axes(9),'Airspeed [m/s]');
legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');
%calculate time differences and plot them
intervals = zeros(0,imaxtime - imintime);
for k = imintime+1:imaxtime
intervals(k) = time(k) - time(k-1);
end
plot(h.axes(10), time(imintime:imaxtime), intervals);
%Set same timescale for all plots
for i=2:NrAxes
set(h.axes(i),'XLim',[mintime maxtime]);
end
set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
end
%% ************************************************************************
% DRAWCURRENTAIRCRAFTSTATE(nested function)
% ************************************************************************
function DrawCurrentAircraftState()
%find current data index
i=find(time>=CurTime,1,'first');
%**********************************************************************
% Current aircraft state label update
%**********************************************************************
acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',...
'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',...
'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),...
', y=',num2str(sysvector.IMU_MagY(i)),...
', z=',num2str(sysvector.IMU_MagZ(i)),']'];
acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),...
', y=',num2str(sysvector.IMU_AccY(i)),...
', z=',num2str(sysvector.IMU_AccZ(i)),']'];
acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),...
', y=',num2str(sysvector.IMU_GyroY(i)),...
', z=',num2str(sysvector.IMU_GyroZ(i)),']'];
acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),...
', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),...
', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']'];
acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');
%for j=1:4
acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),','];
acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),','];
acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),','];
acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),','];
%end
acstate{7,:}=[acstate{7,:},']'];
acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');
%for j=1:8
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),','];
acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),','];
%end
acstate{8,:}=[acstate{8,:},']'];
acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']'];
set(h.edits.AircraftState,'String',acstate);
%**********************************************************************
% GPS Plot Update
%**********************************************************************
%Plot traveled path, and and time.
figure(h.figures(2));
hold on;
if(CurTime>mintime+1) %the +1 is only a small bugfix
h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ...
double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ...
double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2);
end;
hold off
%Plot current position
newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt];
if(numel(h.pathpoints)<=3) %empty path
h.pathpoints(1,1:3)=newpoint;
else %Not empty, append new point
h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;
end
axes(h.axes(1));
line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);
% Plot current time (small label next to current gps position)
textdesc=strcat(' t=',num2str(time(i)),'s');
if(isvalidhandle(h.markertext))
delete(h.markertext); %delete old text
end
h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,...
double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc);
set(h.edits.CurTime,'String',CurTime);
%**********************************************************************
% Plot the lines showing the current time in the 2-d plots
%**********************************************************************
for i=2:NrAxes
if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end
ylims=get(h.axes(i),'YLim');
h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');
set(h.markerline(i),'parent',h.axes(i));
end
set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
end
%% ************************************************************************
% MINMAXTIME CALLBACK (nested function)
% ************************************************************************
function minmaxtime_callback(hObj,event) %#ok<INUSL>
new_mintime=get(h.sliders.MinTime,'Value');
new_maxtime=get(h.sliders.MaxTime,'Value');
%Safety checks:
bErr=false;
%1: mintime must be < maxtime
if((new_mintime>maxtime) || (new_maxtime<mintime))
set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');
bErr=true;
else
%2: MinTime must be <=CurTime
if(new_mintime>CurTime)
set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');
mintime=new_mintime;
CurTime=mintime;
bErr=true;
end
%3: MaxTime must be >CurTime
if(new_maxtime<CurTime)
set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');
maxtime=new_maxtime;
CurTime=maxtime;
bErr=true;
end
end
if(bErr==false)
maxtime=new_maxtime;
mintime=new_mintime;
end
%Needs to be done in case values were reset above
set(h.sliders.MinTime,'Value',mintime);
set(h.sliders.MaxTime,'Value',maxtime);
%Update curtime-slider
set(h.sliders.CurTime,'Value',CurTime);
set(h.sliders.CurTime,'Max',maxtime);
set(h.sliders.CurTime,'Min',mintime);
temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); %Set Stepsize to constant [in seconds]
%update edit fields
set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));
set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));
set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));
%Finally, we have to redraw. Update time indices first.
[imintime,imaxtime]=FindMinMaxTimeIndices();
DrawRawData(); %Rawdata only
DrawCurrentAircraftState(); %path info & markers
end
%% ************************************************************************
% CURTIME CALLBACK (nested function)
% ************************************************************************
function curtime_callback(hObj,event) %#ok<INUSL>
%find current time
if(hObj==h.sliders.CurTime)
CurTime=get(h.sliders.CurTime,'Value');
elseif (hObj==h.edits.CurTime)
temp=str2num(get(h.edits.CurTime,'String'));
if(temp<maxtime && temp>mintime)
CurTime=temp;
else
%Error
set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');
end
else
%Error
set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');
end
set(h.sliders.CurTime,'Value',CurTime);
set(h.edits.CurTime,'String',num2str(CurTime));
%Redraw time markers, but don't have to redraw the whole raw data
DrawCurrentAircraftState();
end
%% ************************************************************************
% FINDMINMAXINDICES (nested function)
% ************************************************************************
function [idxmin,idxmax] = FindMinMaxTimeIndices()
for i=1:size(sysvector.TIME_StartTime,1)
if time(i)>=mintime; idxmin=i; break; end
end
for i=1:size(sysvector.TIME_StartTime,1)
if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end
if time(i)>=maxtime; idxmax=i; break; end
end
mintime=time(idxmin);
maxtime=time(idxmax);
end
%% ************************************************************************
% ISVALIDHANDLE (nested function)
% ************************************************************************
function isvalid = isvalidhandle(handle)
if(exist(varname(handle))>0 && length(ishandle(handle))>0)
if(ishandle(handle)>0)
if(handle>0.0)
isvalid=true;
return;
end
end
end
isvalid=false;
end
%% ************************************************************************
% JUST SOME SMALL HELPER FUNCTIONS (nested function)
% ************************************************************************
function out = varname(var)
out = inputname(1);
end
%This is the end of the matlab file / the main function
end

343
Tools/sdlog2/sdlog2_dump.py
View File

@ -1,343 +0,0 @@
#!/usr/bin/env python
from __future__ import print_function
"""Dump binary log generated by PX4's sdlog2 or APM as CSV
Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[_field1,field2,...]]
Example Usage: python sdlog2_dump.py log.bin -m GPS_Lat,Lng -m AHR2_Roll,Pitch,Yaw
-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
if sys.hexversion >= 0x030000F0:
runningPython3 = True
def _parseCString(cstr):
return str(cstr, 'ascii').split('\0')[0]
else:
runningPython3 = False
def _parseCString(cstr):
return str(cstr).split('\0')[0]
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),
"d": ("d", 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
__file_name = None
__file = None
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 = bytearray() # 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 setFileName(self, file_name):
self.__file_name = file_name
if file_name != None:
self.__file = open(file_name, 'w+')
else:
self.__file = None
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, "rb")
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 = self.__buffer[self.__ptr]
head2 = 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 = 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.get(msg_type, None)
if msg_descr == None:
if self.__correct_errors:
self.__ptr += 1
continue
else:
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
if not self.__debug_out:
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
if self.__file != None:
print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
else:
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)
if self.__file != None:
print(self.__csv_delim.join(s), file=self.__file)
else:
print(self.__csv_delim.join(s))
def __parseMsgDescr(self):
if runningPython3:
data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
else:
data = struct.unpack(self.MSG_FORMAT_STRUCT, str(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 = _parseCString(data[2])
msg_format = _parseCString(data[3])
msg_labels = _parseCString(data[4]).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):
if runningPython3:
data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
else:
data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))
for i in range(len(data)):
if type(data[i]) is str:
data[i] = _parseCString(data[i])
m = msg_mults[i]
if m != None:
data[i] = data[i] * m
if self.__debug_out:
s = []
for i in range(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 range(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")
print("\t-fPrint to file instead of stdout")
return
fn = sys.argv[1]
debug_out = False
correct_errors = False
msg_filter = []
csv_null = ""
csv_delim = ","
time_msg = "TIME"
file_name = 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 == "f":
file_name = 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"
elif arg == "-f":
opt = "f"
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.setFileName(file_name)
parser.setDebugOut(debug_out)
parser.setCorrectErrors(correct_errors)
parser.process(fn)
if __name__ == "__main__":
_main()

1
Tools/uorb_graph/create.py
View File

@ -222,7 +222,6 @@ class Graph:
special_cases_sub = [
('sensors', r'voted_sensors_update\.cpp$', r'\binit_sensor_class\b\(([^,)]+)', r'^meta$'),
('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'),
('sdlog2', r'.*', None, r'^topic$'),
('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),

1
cmake/configs/nuttx_aerocore2_default.cmake
View File

@ -91,7 +91,6 @@ set(config_module_list
# Logging
#
modules/logger
#modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_auav-x21_default.cmake
View File

@ -111,7 +111,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_av-x-v1_default.cmake
View File

@ -103,7 +103,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_mindpx-v2_default.cmake
View File

@ -107,7 +107,6 @@ set(config_module_list
#
# Logging
#
modules/sdlog2
modules/logger
#

1
cmake/configs/nuttx_nxphlite-v3_default.cmake
View File

@ -112,7 +112,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4-same70xplained-v1_default.cmake
View File

@ -84,7 +84,6 @@ set(config_module_list
#
# Logging
#
modules/sdlog2
## modules/logger
#

1
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -134,7 +134,6 @@ set(config_module_list
# Logging
#
modules/logger
#modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4fmu-v2_test.cmake
View File

@ -123,7 +123,6 @@ set(config_module_list
# Logging
#
#modules/logger
#modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4fmu-v3_default.cmake
View File

@ -121,7 +121,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@ -108,7 +108,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
View File

@ -112,7 +112,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4fmu-v5_default.cmake
View File

@ -114,7 +114,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
View File

@ -91,7 +91,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/posix_bebop_default.cmake
View File

@ -52,7 +52,6 @@ set(config_module_list
#
# Library modules
#
modules/sdlog2
modules/logger
modules/commander
modules/dataman

1
cmake/configs/posix_eagle_hil.cmake
View File

@ -29,7 +29,6 @@ set(config_module_list
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander

1
cmake/configs/posix_ocpoc_cross.cmake
View File

@ -55,7 +55,6 @@ set(config_module_list
#
# Library modules
#
modules/sdlog2
modules/logger
modules/commander
modules/dataman

1
cmake/configs/posix_ocpoc_ubuntu.cmake
View File

@ -55,7 +55,6 @@ set(config_module_list
#
# Library modules
#
modules/sdlog2
modules/logger
modules/commander
modules/dataman

1
cmake/configs/posix_rpi_common.cmake
View File

@ -66,7 +66,6 @@ set(config_module_list
#
# Library modules
#
modules/sdlog2
modules/logger
modules/commander
modules/dataman

1
cmake/configs/posix_sdflight_default.cmake
View File

@ -58,7 +58,6 @@ set(config_module_list
modules/muorb/krait
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander

1
cmake/configs/posix_sdflight_legacy.cmake
View File

@ -49,7 +49,6 @@ set(config_module_list
modules/muorb/krait
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander

1
cmake/configs/posix_sitl_default.cmake
View File

@ -95,7 +95,6 @@ set(config_module_list
# Logging
#
modules/logger
modules/sdlog2
#
# Library modules

1
cmake/configs/posix_sitl_replay.cmake
View File

@ -8,7 +8,6 @@ set(config_module_list
systemcmds/perf
modules/ekf2
modules/ekf2_replay
modules/sdlog2
modules/logger
)

2
posix-configs/SITL/init/ekf2/rover
View File

@ -78,6 +78,6 @@ mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
sdlog2 start -r 200 -e -t -a
logger start -e -t
mavlink boot_complete
replay trystart

2
posix-configs/SITL/init/ekf2/standard_vtol
View File

@ -68,7 +68,7 @@ accelsim start
barosim start
gpssim start
measairspeedsim start
pwm_out_sim start start
pwm_out_sim start
sensors start
commander start
land_detector start vtol

2
posix-configs/SITL/init/lpe/rover
View File

@ -74,6 +74,6 @@ mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
sdlog2 start -r 200 -e -t -a
logger start -e -t
mavlink boot_complete
replay trystart

46
src/modules/sdlog2/CMakeLists.txt
View File

@ -1,46 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE modules__sdlog2
MAIN sdlog2
PRIORITY "SCHED_PRIORITY_MAX-30"
STACK_MAIN 1200
STACK_MAX 1600
COMPILE_FLAGS
-Wno-sign-compare # TODO: fix all sign-compare
SRCS
sdlog2.c
logbuffer.c
DEPENDS
)

165
src/modules/sdlog2/logbuffer.c
View File

@ -1,165 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file logbuffer.c
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#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 = NULL;
lb->perf_dropped = perf_alloc(PC_COUNT, "sd drop");
return PX4_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)
{
// allocate buffer if not yet present
if (lb->data == NULL) {
lb->data = malloc(lb->size);
}
// allocation failed, bail out
if (lb->data == NULL) {
return false;
}
// bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1;
if (available < 0) {
available += lb->size;
}
if (size > available) {
// buffer overflow
perf_count(lb->perf_dropped);
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 = lb->write_ptr > 0;
}
*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;
}
void logbuffer_free(struct logbuffer_s *lb)
{
if (lb->data) {
free(lb->data);
lb->write_ptr = 0;
lb->read_ptr = 0;
lb->data = NULL;
perf_free(lb->perf_dropped);
}
}
void logbuffer_reset(struct logbuffer_s *lb)
{
// Keep the buffer but reset the pointers.
lb->write_ptr = 0;
lb->read_ptr = 0;
}

74
src/modules/sdlog2/logbuffer.h
View File

@ -1,74 +0,0 @@
/****************************************************************************
*
* 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 logbuffer.h
*
* Ring FIFO buffer for binary log data.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_RINGBUFFER_H_
#define SDLOG2_RINGBUFFER_H_
#include <stdbool.h>
#include <perf/perf_counter.h>
struct logbuffer_s {
// pointers and size are in bytes
int write_ptr;
int read_ptr;
int size;
char *data;
perf_counter_t perf_dropped;
};
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);
void logbuffer_free(struct logbuffer_s *lb);
void logbuffer_reset(struct logbuffer_s *lb);
#endif

95
src/modules/sdlog2/params.c
View File

@ -1,95 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 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.
*
****************************************************************************/
/**
* Logging rate.
*
* A value of -1 indicates the commandline argument
* should be obeyed. A value of 0 sets the minimum rate,
* any other value is interpreted as rate in Hertz. This
* parameter is only read out before logging starts (which
* commonly is before arming).
*
* @unit Hz
* @min -1
* @max 250
* @group SD Logging
*/
PARAM_DEFINE_INT32(SDLOG_RATE, -1);
/**
* Extended logging mode
*
* A value of -1 indicates the command line argument
* should be obeyed. A value of 0 disables extended
* logging mode, a value of 1 enables it. This
* parameter is only read out before logging starts
* (which commonly is before arming).
*
* @min -1
* @max 1
* @value -1 Command Line
* @value 0 Disable
* @value 1 Enable
* @group SD Logging
*/
PARAM_DEFINE_INT32(SDLOG_EXT, -1);
/**
* Use timestamps only if GPS 3D fix is available
*
* Constrain the log folder creation
* to only use the time stamp if a 3D GPS lock is
* present.
*
* @boolean
* @group SD Logging
*/
PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);
/**
* Give logging app higher thread priority to avoid data loss.
* This is used for gathering replay logs for the ekf2 module.
*
* A value of 0 indicates that the default priority is used.
* Increasing the parameter in steps of one increases the priority.
*
* @min 0
* @max 3
* @value 0 Low priority
* @value 1 Default priority
* @value 2 Medium priority
* @value 3 Max priority
* @group SD Logging
*/
PARAM_DEFINE_INT32(SDLOG_PRIO_BOOST, 2);

2260
src/modules/sdlog2/sdlog2.c
View File

File diff suppressed because it is too large Load Diff

106
src/modules/sdlog2/sdlog2_format.h
View File

@ -1,106 +0,0 @@
/****************************************************************************
*
* 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 sdlog2_format.h
*
* General log format structures and macro.
*
* @author Anton Babushkin <anton.babushkin@me.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_S(_name, _struct_name, _format, _labels) { \
.type = LOG_##_name##_MSG, \
.length = sizeof(struct log_##_struct_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_ */

656
src/modules/sdlog2/sdlog2_messages.h
View File

@ -1,656 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sdlog2_messages.h
*
* Log messages and structures definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Roman Bapst <roman@px4.io>
*/
#ifndef SDLOG2_MESSAGES_H_
#define SDLOG2_MESSAGES_H_
#include "sdlog2_format.h"
/* define message formats */
#pragma pack(push, 1)
/* --- ATT - ATTITUDE --- */
#define LOG_ATT_MSG 2
struct log_ATT_s {
float q_w;
float q_x;
float q_y;
float q_z;
float roll;
float pitch;
float yaw;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* --- ATSP - ATTITUDE SET POINT --- */
#define LOG_ATSP_MSG 3
struct log_ATSP_s {
float roll_sp;
float pitch_sp;
float yaw_sp;
float thrust_sp;
float q_w;
float q_x;
float q_y;
float q_z;
};
/* --- IMU - IMU SENSORS --- */
#define LOG_IMU_MSG 4
#define LOG_IMU1_MSG 22
#define LOG_IMU2_MSG 23
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;
float temp_acc;
float temp_gyro;
float temp_mag;
};
/* --- SENS - OTHER SENSORS --- */
#define LOG_SENS_MSG 5
struct log_SENS_s {
float baro_pres;
float baro_alt;
float baro_temp;
};
/* --- LPOS - LOCAL POSITION --- */
#define LOG_LPOS_MSG 6
struct log_LPOS_s {
float x;
float y;
float z;
float ground_dist;
float ground_dist_rate;
float vx;
float vy;
float vz;
int32_t ref_lat;
int32_t ref_lon;
float ref_alt;
uint8_t pos_flags;
uint8_t ground_dist_flags;
float eph;
float epv;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
#define LOG_LPSP_MSG 7
struct log_LPSP_s {
float x;
float y;
float z;
float yaw;
float vx;
float vy;
float vz;
float acc_x;
float acc_y;
float acc_z;
};
/* --- GPS - GPS POSITION --- */
#define LOG_GPS_MSG 8
#define LOG_DGPS_MSG 58
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;
uint8_t sats;
uint16_t snr_mean;
uint16_t noise_per_ms;
uint16_t jamming_indicator;
};
/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
#define LOG_ATTC_MSG 9
#define LOG_ATC1_MSG 46
struct log_ATTC_s {
float roll;
float pitch;
float yaw;
float thrust;
};
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
uint8_t main_state;
uint8_t nav_state;
uint8_t arming_state;
uint8_t failsafe;
uint8_t is_rot_wing;
};
/* --- RC - RC INPUT CHANNELS --- */
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[12];
uint8_t rssi;
uint8_t channel_count;
uint8_t signal_lost;
uint32_t frame_drop;
};
/* --- OUT - ACTUATOR OUTPUT --- */
#define LOG_OUT0_MSG 12
struct log_OUT_s {
float output[8];
};
/* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 13
struct log_AIRS_s {
float indicated_airspeed_m_s;
float true_airspeed_m_s;
float true_airspeed_unfiltered_m_s;
float air_temperature_celsius;
float confidence;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 14
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
float yaw_rate_sp;
};
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
uint8_t sensor_id;
float pixel_flow_x_integral;
float pixel_flow_y_integral;
float gyro_x_rate_integral;
float gyro_y_rate_integral;
float gyro_z_rate_integral;
float ground_distance_m;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t frame_count_since_last_readout;
int16_t gyro_temperature;
uint8_t quality;
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
#define LOG_GPOS_MSG 16
struct log_GPOS_s {
int32_t lat;
int32_t lon;
float alt;
float vel_n;
float vel_e;
float vel_d;
float eph;
float epv;
float terrain_alt;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
int32_t lat;
int32_t lon;
float alt;
float yaw;
uint8_t type;
float loiter_radius;
int8_t loiter_direction;
float pitch_min;
};
/* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 18
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_num;
uint16_t esc_address;
uint16_t esc_version;
float esc_voltage;
float esc_current;
int32_t esc_rpm;
float esc_temperature;
float esc_setpoint;
uint16_t esc_setpoint_raw;
};
/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
#define LOG_GVSP_MSG 19
struct log_GVSP_s {
float vx;
float vy;
float vz;
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
struct log_BATT_s {
float voltage;
float voltage_filtered;
float current;
float current_filtered;
float discharged;
float remaining;
float scale;
uint8_t warning;
};
/* --- DIST - RANGE SENSOR DISTANCE --- */
#define LOG_DIST_MSG 21
struct log_DIST_s {
uint8_t id;
uint8_t type;
uint8_t orientation;
float current_distance;
float covariance;
};
/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
struct log_PWR_s {
float peripherals_5v;
float servo_rail_5v;
float servo_rssi;
uint8_t usb_ok;
uint8_t brick_ok;
uint8_t servo_ok;
uint8_t low_power_rail_overcurrent;
uint8_t high_power_rail_overcurrent;
};
/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
#define LOG_MOCP_MSG 25
struct log_MOCP_s {
float qw;
float qx;
float qy;
float qz;
float x;
float y;
float z;
};
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
#define LOG_GS0A_MSG 26
struct log_GS0A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
#define LOG_GS0B_MSG 27
struct log_GS0B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
#define LOG_GS1A_MSG 28
struct log_GS1A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
#define LOG_GS1B_MSG 29
struct log_GS1B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- TECS - TECS STATUS --- */
#define LOG_TECS_MSG 30
struct log_TECS_s {
float altitudeSp;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float airspeedSp;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
float totalEnergyError;
float totalEnergyRateError;
float energyDistributionError;
float energyDistributionRateError;
float pitch_integ;
float throttle_integ;
uint8_t mode;
};
/* --- WIND - WIND ESTIMATE --- */
#define LOG_WIND_MSG 31
struct log_WIND_s {
float x;
float y;
float cov_x;
float cov_y;
};
/* --- EST0 - ESTIMATOR STATUS --- */
#define LOG_EST0_MSG 32
struct log_EST0_s {
float s[12];
uint8_t n_states;
uint8_t nan_flags;
uint16_t fault_flags;
uint8_t timeout_flags;
};
/* --- EST1 - ESTIMATOR STATUS --- */
#define LOG_EST1_MSG 33
struct log_EST1_s {
float s[16];
};
/* --- EST2 - ESTIMATOR STATUS --- */
#define LOG_EST2_MSG 34
struct log_EST2_s {
float cov[12];
uint16_t gps_check_fail_flags;
uint16_t control_mode_flags;
uint8_t health_flags;
uint16_t innov_test_flags;
};
/* --- EST3 - ESTIMATOR STATUS --- */
#define LOG_EST3_MSG 35
struct log_EST3_s {
float cov[16];
};
/* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 36
#define LOG_TEL1_MSG 37
#define LOG_TEL2_MSG 38
#define LOG_TEL3_MSG 39
struct log_TEL_s {
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
uint64_t heartbeat_time;
};
/* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 40
struct log_VISN_s {
float x;
float y;
float z;
float vx;
float vy;
float vz;
float qw;
float qx;
float qy;
float qz;
};
/* --- ENCODERS - ENCODER DATA --- */
#define LOG_ENCD_MSG 41
struct log_ENCD_s {
int64_t cnt0;
float vel0;
int64_t cnt1;
float vel1;
};
/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
#define LOG_AIR1_MSG 42
/* --- VTOL - VTOL VEHICLE STATUS */
#define LOG_VTOL_MSG 43
struct log_VTOL_s {
uint8_t rw_mode;
uint8_t trans_mode;
uint8_t failsafe_mode;
};
/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
#define LOG_TSYN_MSG 44
struct log_TSYN_s {
uint64_t time_offset;
};
/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
#define LOG_MACS_MSG 45
struct log_MACS_s {
float roll_rate_integ;
float pitch_rate_integ;
float yaw_rate_integ;
};
/* WARNING: ID 46 is already in use for ATTC1 */
/* --- CONTROL STATE --- */
#define LOG_CTS_MSG 47
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* --- EST4 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST4_MSG 48
struct log_EST4_s {
float s[15];
};
/* --- EST5 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST5_MSG 49
struct log_EST5_s {
float s[12];
};
#define LOG_OUT1_MSG 50
/* --- EST6 - ESTIMATOR INNOVATIONS --- */
#define LOG_EST6_MSG 53
struct log_EST6_s {
float s[6];
};
/* --- CAMERA TRIGGER --- */
#define LOG_CAMT_MSG 55
struct log_CAMT_s {
uint64_t timestamp;
uint32_t seq;
};
/* --- LAND DETECTOR --- */
#define LOG_LAND_MSG 57
struct log_LAND_s {
uint8_t landed;
};
/* 58 used for DGPS message
shares struct with GPS MSG 8*/
/* --- SYSTEM LOAD --- */
#define LOG_LOAD_MSG 61
struct log_LOAD_s {
float cpu_load;
};
/* --- DPRS - DIFFERENTIAL PRESSURE --- */
#define LOG_DPRS_MSG 62
struct log_DPRS_s {
uint64_t error_count;
float differential_pressure_raw_pa;
float differential_pressure_filtered_pa;
float temperature;
};
/* --- LOW STACK --- */
#define LOG_STCK_MSG 63
struct log_STCK_s {
char task_name[16];
uint16_t stack_free;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
uint64_t t;
};
/* --- VER - VERSION --- */
#define LOG_VER_MSG 130
struct log_VER_s {
char arch[16];
char fw_git[64];
};
/* --- PARM - PARAMETER --- */
#define LOG_PARM_MSG 131
struct log_PARM_s {
char name[64];
float value;
};
#pragma pack(pop)
// the lower type of initialisation is not supported in C++
#ifndef __cplusplus
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "ffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATSP, "ffffffff", "RollSP,PitchSP,YawSP,ThrustSP,qw,qx,qy,qz"),
LOG_FORMAT_S(IMU, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"),
LOG_FORMAT_S(IMU1, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"),
LOG_FORMAT_S(IMU2, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"),
LOG_FORMAT_S(SENS, SENS, "fff", "BaroPres,BaroAlt,BaroTemp"),
LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"),
LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffffffffff", "X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"),
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(DGPS, GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBB", "MainState,NavState,ArmS,Failsafe,IsRotWing"),
LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
LOG_FORMAT_S(OUT0, OUT, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT_S(OUT1, OUT, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fffff", "IAS,TAS,TASraw,Temp,Confidence"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "fffffffB", "V,VFilt,C,CFilt,Discharged,Remaining,Scale,Warning"),
LOG_FORMAT(DIST, "BBBff", "Id,Type,Orientation,Distance,Covariance"),
LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT(EST0, "ffffffffffffBBHB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"),
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(EST2, "ffffffffffffHHBH", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL,fHealth,IC"),
LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
LOG_FORMAT(EST4, "fffffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV,e1,e2,e3"),
LOG_FORMAT(EST5, "ffffffffffff", "MaxI,MayI,MazI,MaxIV,MayIV,MazIV,HeI,HeIV,AiI,AiIV,BeI,BeIV"),
LOG_FORMAT(EST6, "ffffff", "FxI,FyI,FxIV,FyIV,HAGLI,HAGLIV"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,ERE,EDE,EDRE,PtchI,ThrI,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"),
LOG_FORMAT(CAMT, "QI", "timestamp,seq"),
LOG_FORMAT(LAND, "B", "Landed"),
LOG_FORMAT(LOAD, "f", "CPU"),
LOG_FORMAT(DPRS, "Qfff", "errors,DPRESraw,DPRES,Temp"),
LOG_FORMAT(STCK, "NH", "Task,Free"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
LOG_FORMAT(PARM, "Zf", "Name,Value")
};
static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
#endif
#endif /* SDLOG2_MESSAGES_H_ */

12
src/modules/systemlib/system_params.c
View File

@ -152,18 +152,6 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600);
*/
PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
/**
* SD logger
*
* @value 0 sdlog2 (legacy)
* @value 1 logger (default)
* @min 0
* @max 1
* @reboot_required true
* @group System
*/
PARAM_DEFINE_INT32(SYS_LOGGER, 1);
/**
* Enable stack checking
*