From 8537863848a471e95eaa4719bb6622c4819c9ae3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 12 Jul 2018 17:21:18 -0400 Subject: [PATCH] delete sdlog2 --- ROMFS/px4fmu_common/init.d/rc.logging | 40 +- Tools/astyle/files_to_check_code_style.sh | 1 - Tools/sdlog2/README.md | 29 - Tools/sdlog2/geo_tag_images.py | 434 ---- Tools/sdlog2/geotagging.py | 237 -- Tools/sdlog2/logconv.m | 535 ---- Tools/sdlog2/sdlog2_dump.py | 343 --- Tools/uorb_graph/create.py | 1 - cmake/configs/nuttx_aerocore2_default.cmake | 1 - cmake/configs/nuttx_auav-x21_default.cmake | 1 - cmake/configs/nuttx_av-x-v1_default.cmake | 1 - cmake/configs/nuttx_mindpx-v2_default.cmake | 1 - cmake/configs/nuttx_nxphlite-v3_default.cmake | 1 - .../nuttx_px4-same70xplained-v1_default.cmake | 1 - cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 - cmake/configs/nuttx_px4fmu-v2_test.cmake | 1 - cmake/configs/nuttx_px4fmu-v3_default.cmake | 1 - cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 - .../configs/nuttx_px4fmu-v4pro_default.cmake | 1 - cmake/configs/nuttx_px4fmu-v5_default.cmake | 1 - .../nuttx_px4nucleoF767ZI-v1_default.cmake | 1 - cmake/configs/posix_bebop_default.cmake | 1 - cmake/configs/posix_eagle_hil.cmake | 1 - cmake/configs/posix_ocpoc_cross.cmake | 1 - cmake/configs/posix_ocpoc_ubuntu.cmake | 1 - cmake/configs/posix_rpi_common.cmake | 1 - cmake/configs/posix_sdflight_default.cmake | 1 - cmake/configs/posix_sdflight_legacy.cmake | 1 - cmake/configs/posix_sitl_default.cmake | 1 - cmake/configs/posix_sitl_replay.cmake | 1 - posix-configs/SITL/init/ekf2/rover | 2 +- posix-configs/SITL/init/ekf2/standard_vtol | 2 +- posix-configs/SITL/init/lpe/rover | 2 +- src/modules/sdlog2/CMakeLists.txt | 46 - src/modules/sdlog2/logbuffer.c | 165 -- src/modules/sdlog2/logbuffer.h | 74 - src/modules/sdlog2/params.c | 95 - src/modules/sdlog2/sdlog2.c | 2260 ----------------- src/modules/sdlog2/sdlog2_format.h | 106 - src/modules/sdlog2/sdlog2_messages.h | 656 ----- src/modules/systemlib/system_params.c | 12 - 41 files changed, 20 insertions(+), 5042 deletions(-) delete mode 100644 Tools/sdlog2/README.md delete mode 100644 Tools/sdlog2/geo_tag_images.py delete mode 100644 Tools/sdlog2/geotagging.py delete mode 100644 Tools/sdlog2/logconv.m delete mode 100755 Tools/sdlog2/sdlog2_dump.py delete mode 100644 src/modules/sdlog2/CMakeLists.txt delete mode 100644 src/modules/sdlog2/logbuffer.c delete mode 100644 src/modules/sdlog2/logbuffer.h delete mode 100644 src/modules/sdlog2/params.c delete mode 100644 src/modules/sdlog2/sdlog2.c delete mode 100644 src/modules/sdlog2/sdlog2_format.h delete mode 100644 src/modules/sdlog2/sdlog2_messages.h diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 909bcc4b1a..5ef8ca464d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -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 diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index cf1be67d09..cc3226b4d7 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -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 diff --git a/Tools/sdlog2/README.md b/Tools/sdlog2/README.md deleted file mode 100644 index 18aba70030..0000000000 --- a/Tools/sdlog2/README.md +++ /dev/null @@ -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. diff --git a/Tools/sdlog2/geo_tag_images.py b/Tools/sdlog2/geo_tag_images.py deleted file mode 100644 index a3ec07e7f3..0000000000 --- a/Tools/sdlog2/geo_tag_images.py +++ /dev/null @@ -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() diff --git a/Tools/sdlog2/geotagging.py b/Tools/sdlog2/geotagging.py deleted file mode 100644 index 564737d663..0000000000 --- a/Tools/sdlog2/geotagging.py +++ /dev/null @@ -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() diff --git a/Tools/sdlog2/logconv.m b/Tools/sdlog2/logconv.m deleted file mode 100644 index e19c97fa3c..0000000000 --- a/Tools/sdlog2/logconv.m +++ /dev/null @@ -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 - 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_maxtimeCurTime) - 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 - %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(tempmintime) - 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 diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py deleted file mode 100755 index 4cc2100224..0000000000 --- a/Tools/sdlog2/sdlog2_dump.py +++ /dev/null @@ -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 [-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 [-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() diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 37ca39ebd0..938fdc3aa1 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -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\]$'), diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake index 1bdbe99cad..7b758cca98 100644 --- a/cmake/configs/nuttx_aerocore2_default.cmake +++ b/cmake/configs/nuttx_aerocore2_default.cmake @@ -91,7 +91,6 @@ set(config_module_list # Logging # modules/logger - #modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_auav-x21_default.cmake b/cmake/configs/nuttx_auav-x21_default.cmake index 337d1f7128..6d50633770 100644 --- a/cmake/configs/nuttx_auav-x21_default.cmake +++ b/cmake/configs/nuttx_auav-x21_default.cmake @@ -111,7 +111,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_av-x-v1_default.cmake b/cmake/configs/nuttx_av-x-v1_default.cmake index 795de9429b..a088736ae0 100644 --- a/cmake/configs/nuttx_av-x-v1_default.cmake +++ b/cmake/configs/nuttx_av-x-v1_default.cmake @@ -103,7 +103,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake index 63658b2a09..63fc438cd0 100644 --- a/cmake/configs/nuttx_mindpx-v2_default.cmake +++ b/cmake/configs/nuttx_mindpx-v2_default.cmake @@ -107,7 +107,6 @@ set(config_module_list # # Logging # - modules/sdlog2 modules/logger # diff --git a/cmake/configs/nuttx_nxphlite-v3_default.cmake b/cmake/configs/nuttx_nxphlite-v3_default.cmake index e681e85fc7..9b29c4986c 100644 --- a/cmake/configs/nuttx_nxphlite-v3_default.cmake +++ b/cmake/configs/nuttx_nxphlite-v3_default.cmake @@ -112,7 +112,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake b/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake index 20cde30201..942a5252f7 100644 --- a/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake +++ b/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake @@ -84,7 +84,6 @@ set(config_module_list # # Logging # - modules/sdlog2 ## modules/logger # diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index b53d349e9f..f28a8c9219 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -134,7 +134,6 @@ set(config_module_list # Logging # modules/logger - #modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake index cf990f3c0e..bcc1b2a82b 100644 --- a/cmake/configs/nuttx_px4fmu-v2_test.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake @@ -123,7 +123,6 @@ set(config_module_list # Logging # #modules/logger - #modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake index 2344282420..5ba093a764 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -121,7 +121,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index c665bf3f22..02729e2554 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -108,7 +108,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake index 8985fdfd0c..1fde0ee72b 100644 --- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake @@ -112,7 +112,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index 0957a119e3..114b5ae0ac 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -114,7 +114,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake index 2172366f41..49f49b32d6 100644 --- a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake +++ b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake @@ -91,7 +91,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake index 7a9f5cc401..a31e0e3fd1 100644 --- a/cmake/configs/posix_bebop_default.cmake +++ b/cmake/configs/posix_bebop_default.cmake @@ -52,7 +52,6 @@ set(config_module_list # # Library modules # - modules/sdlog2 modules/logger modules/commander modules/dataman diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake index aa536730ec..a06450b1d6 100644 --- a/cmake/configs/posix_eagle_hil.cmake +++ b/cmake/configs/posix_eagle_hil.cmake @@ -29,7 +29,6 @@ set(config_module_list modules/sensors modules/dataman - modules/sdlog2 modules/logger modules/simulator modules/commander diff --git a/cmake/configs/posix_ocpoc_cross.cmake b/cmake/configs/posix_ocpoc_cross.cmake index 702c2bbd80..7471e4f920 100644 --- a/cmake/configs/posix_ocpoc_cross.cmake +++ b/cmake/configs/posix_ocpoc_cross.cmake @@ -55,7 +55,6 @@ set(config_module_list # # Library modules # - modules/sdlog2 modules/logger modules/commander modules/dataman diff --git a/cmake/configs/posix_ocpoc_ubuntu.cmake b/cmake/configs/posix_ocpoc_ubuntu.cmake index 4738e78eb9..8f3b549c91 100644 --- a/cmake/configs/posix_ocpoc_ubuntu.cmake +++ b/cmake/configs/posix_ocpoc_ubuntu.cmake @@ -55,7 +55,6 @@ set(config_module_list # # Library modules # - modules/sdlog2 modules/logger modules/commander modules/dataman diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index fd87c2696b..c04108b261 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -66,7 +66,6 @@ set(config_module_list # # Library modules # - modules/sdlog2 modules/logger modules/commander modules/dataman diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake index ad4e87d245..70e1377317 100644 --- a/cmake/configs/posix_sdflight_default.cmake +++ b/cmake/configs/posix_sdflight_default.cmake @@ -58,7 +58,6 @@ set(config_module_list modules/muorb/krait modules/sensors modules/dataman - modules/sdlog2 modules/logger modules/simulator modules/commander diff --git a/cmake/configs/posix_sdflight_legacy.cmake b/cmake/configs/posix_sdflight_legacy.cmake index 60cb403e74..3de331b104 100644 --- a/cmake/configs/posix_sdflight_legacy.cmake +++ b/cmake/configs/posix_sdflight_legacy.cmake @@ -49,7 +49,6 @@ set(config_module_list modules/muorb/krait modules/sensors modules/dataman - modules/sdlog2 modules/logger modules/simulator modules/commander diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index b25b4afed3..0c3a3fb3e5 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -95,7 +95,6 @@ set(config_module_list # Logging # modules/logger - modules/sdlog2 # # Library modules diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake index 15b7f5da80..54fe6fd07e 100644 --- a/cmake/configs/posix_sitl_replay.cmake +++ b/cmake/configs/posix_sitl_replay.cmake @@ -8,7 +8,6 @@ set(config_module_list systemcmds/perf modules/ekf2 modules/ekf2_replay - modules/sdlog2 modules/logger ) diff --git a/posix-configs/SITL/init/ekf2/rover b/posix-configs/SITL/init/ekf2/rover index e08a77a664..8bf29c99be 100644 --- a/posix-configs/SITL/init/ekf2/rover +++ b/posix-configs/SITL/init/ekf2/rover @@ -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 diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index 244e40b167..ac5216411b 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -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 diff --git a/posix-configs/SITL/init/lpe/rover b/posix-configs/SITL/init/lpe/rover index 52baafb8d0..c17b06d9cf 100644 --- a/posix-configs/SITL/init/lpe/rover +++ b/posix-configs/SITL/init/lpe/rover @@ -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 diff --git a/src/modules/sdlog2/CMakeLists.txt b/src/modules/sdlog2/CMakeLists.txt deleted file mode 100644 index a7e3091681..0000000000 --- a/src/modules/sdlog2/CMakeLists.txt +++ /dev/null @@ -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 - ) - diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c deleted file mode 100644 index 9908b574a0..0000000000 --- a/src/modules/sdlog2/logbuffer.c +++ /dev/null @@ -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 - */ - -#include -#include -#include - -#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; -} diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h deleted file mode 100644 index ba3935513b..0000000000 --- a/src/modules/sdlog2/logbuffer.h +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 - */ - -#ifndef SDLOG2_RINGBUFFER_H_ -#define SDLOG2_RINGBUFFER_H_ - -#include -#include - -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 diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c deleted file mode 100644 index 0e9f49f950..0000000000 --- a/src/modules/sdlog2/params.c +++ /dev/null @@ -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); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c deleted file mode 100644 index 1dea6ef7ef..0000000000 --- a/src/modules/sdlog2/sdlog2.c +++ /dev/null @@ -1,2260 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 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.c - * - * Simple SD logger for flight data. Buffers new sensor values and - * does the heavy SD I/O in a low-priority worker thread. - * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Ban Siesta - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef __PX4_DARWIN -#include -#include -#else -#include -#endif -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "logbuffer.h" -#include "sdlog2_format.h" -#include "sdlog2_messages.h" - -#define PX4_EPOCH_SECS 1234567890L - -#define LOGBUFFER_WRITE_AND_COUNT(_msg) pthread_mutex_lock(&logbuffer_mutex); \ - if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ - log_msgs_written++; \ - } else { \ - log_msgs_skipped++; \ - } \ - pthread_mutex_unlock(&logbuffer_mutex); - -#define SDLOG_MIN(X,Y) ((X) < (Y) ? (X) : (Y)) - -static bool main_thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ -static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ -static const int LOG_BUFFER_SIZE_DEFAULT = 8192; - -#if defined __PX4_POSIX -static const int MAX_WRITE_CHUNK = 2048; -static const int MIN_BYTES_TO_WRITE = 512; -#else -static const int MAX_WRITE_CHUNK = 512; -static const int MIN_BYTES_TO_WRITE = 512; -#endif - -static bool _extended_logging = false; -static bool _gpstime_only = false; -static int32_t _utc_offset = 0; - -#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) -#define MOUNTPOINT PX4_ROOTFSDIR"/fs/microsd" -#else -#define MOUNTPOINT "/root" -#endif -static const char *mountpoint = MOUNTPOINT; -static const char *log_root = MOUNTPOINT "/log"; -static orb_advert_t mavlink_log_pub = NULL; -struct logbuffer_s lb; - -/* mutex / condition to synchronize threads */ -static pthread_mutex_t logbuffer_mutex; -static pthread_cond_t logbuffer_cond; - -#ifdef __PX4_NUTTX -#define LOG_BASE_PATH_LEN 64 -#else -#define LOG_BASE_PATH_LEN 256 -#endif - -static char log_dir[LOG_BASE_PATH_LEN]; - -/* statistics counters */ -static uint64_t start_time = 0; -static unsigned long log_bytes_written = 0; -static unsigned long last_checked_bytes_written = 0; -static unsigned long log_msgs_written = 0; -static unsigned long log_msgs_skipped = 0; - -/* GPS time, used for log files naming */ -static uint64_t gps_time_sec = 0; -static bool has_gps_3d_fix = false; - -/* current state of logging */ -static bool logging_enabled = false; -/* use date/time for naming directories and files (-t option) */ -static bool log_name_timestamp = false; - -/* helper flag to track system state changes */ -static bool flag_system_armed = false; - -/* flag if warning about MicroSD card being almost full has already been sent */ -static bool space_warning_sent = false; - -static pthread_t logwriter_pthread = 0; -static pthread_attr_t logwriter_attr; - -static perf_counter_t perf_write; - -/* Keep track if we've already created a folder named sessXXX because - * we don't want to create yet another one. */ -static bool sess_folder_created = false; - -/** - * Log buffer writing thread. Open and close file here. - */ -static void *logwriter_thread(void *arg); - -/** - * SD log management function. - */ -__EXPORT int sdlog2_main(int argc, char *argv[]); - -static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer); -static bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer); - -/** - * Mainloop of sd log deamon. - */ -int sdlog2_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void sdlog2_usage(const char *reason); - -/** - * Print the current status. - */ -static void sdlog2_status(void); - -/** - * Start logging: create new file and start log writer thread. - */ -static void sdlog2_start_log(void); - -/** - * Stop logging: stop log writer thread and close log file. - */ -static void sdlog2_stop_log(void); - -/** - * Write a header to log file: list of message formats. - */ -static int write_formats(int fd); - -/** - * Write version message to log file. - */ -static int write_version(int fd); - -/** - * Write parameters to log file. - */ -static int write_parameters(int fd); - -static bool file_exist(const char *filename); - -/** - * Check if there is still free space available - */ -static int check_free_space(void); - -static void handle_command(struct vehicle_command_s *cmd); - -static void handle_status(struct vehicle_status_s *cmd); - -/** - * Create dir for current logging session. Store dir name in 'log_dir'. - */ -static int create_log_dir(void); - -/** - * Get the time struct from the currently preferred time source - */ -static bool get_log_time_tt(struct tm *tt, bool boot_time); - -/** - * Select first free log file name and open it. - */ -static int open_log_file(void); - -static int open_perf_file(const char* str); - -static void -sdlog2_usage(const char *reason) -{ - if (reason) { - fprintf(stderr, "%s\n", reason); - } - - PX4_WARN("usage: sdlog2 {start|stop|status|on|off} [-r ] [-b ] -e -a -t -x\n" - "\t-r\tLog rate in Hz, 0 means unlimited rate\n" - "\t-b\tLog buffer size in KiB, default is 8\n" - "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n" - "\t-x\tExtended logging"); -} - -/** - * The logger deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn(). - */ -int sdlog2_main(int argc, char *argv[]) -{ - if (argc < 2) { - sdlog2_usage("missing command"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - PX4_WARN("already running"); - /* this is not an error */ - return 0; - } - - // get sdlog priority boost parameter. This can be used to avoid message drops - // in the log file. However, it considered to be used only for developers. - param_t prio_boost_handle = param_find("SDLOG_PRIO_BOOST"); - int prio_boost = 0; - param_get(prio_boost_handle, &prio_boost); - int task_priority = SCHED_PRIORITY_DEFAULT - 30; - - switch(prio_boost) { - case 1: - task_priority = SCHED_PRIORITY_DEFAULT; - break; - case 2: - task_priority = SCHED_PRIORITY_DEFAULT + (SCHED_PRIORITY_MAX - SCHED_PRIORITY_DEFAULT) / 2; - break; - case 3: - task_priority = SCHED_PRIORITY_MAX; - break; - default: - // use default priority already set above - break; - } - - main_thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - task_priority, - 3400, - sdlog2_thread_main, - (char * const *)argv); - - /* wait for the task to launch */ - unsigned const max_wait_us = 1000000; - unsigned const max_wait_steps = 2000; - - unsigned i; - for (i = 0; i < max_wait_steps; i++) { - usleep(max_wait_us / max_wait_steps); - if (thread_running) { - break; - } - } - - return !(i < max_wait_steps); - } - - if (!strcmp(argv[1], "stop")) { - if (!thread_running) { - PX4_WARN("not started"); - } - - main_thread_should_exit = true; - return 0; - } - - if (!thread_running) { - PX4_WARN("not started\n"); - return 1; - } - - if (!strcmp(argv[1], "status")) { - sdlog2_status(); - return 0; - } - - if (!strncmp(argv[1], "on", 2)) { - struct vehicle_command_s cmd; - - memset(&cmd, 0, sizeof(cmd)); - cmd.command = VEHICLE_COMMAND_VEHICLE_CMD_PREFLIGHT_STORAGE; - cmd.param1 = -1; - cmd.param2 = -1; - cmd.param3 = 1; - cmd.timestamp = hrt_absolute_time(); - orb_advertise(ORB_ID(vehicle_command), &cmd); - return 0; - } - - if (!strcmp(argv[1], "off")) { - struct vehicle_command_s cmd; - - memset(&cmd, 0, sizeof(cmd)); - cmd.command = VEHICLE_COMMAND_VEHICLE_CMD_PREFLIGHT_STORAGE; - cmd.param1 = -1; - cmd.param2 = -1; - cmd.param3 = 2; - cmd.timestamp = hrt_absolute_time(); - orb_advertise(ORB_ID(vehicle_command), &cmd); - return 0; - } - - sdlog2_usage("unrecognized command"); - return 1; -} - -bool get_log_time_tt(struct tm *tt, bool boot_time) { - struct timespec ts; - px4_clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ - time_t utc_time_sec = 0; - - if (_gpstime_only && has_gps_3d_fix) { - utc_time_sec = gps_time_sec; - } else { - utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - } - - if (utc_time_sec > PX4_EPOCH_SECS) { - /* strip the time elapsed since boot */ - if (boot_time) { - utc_time_sec -= hrt_absolute_time() / 1e6; - } - - /* apply utc offset (min, not hour) */ - utc_time_sec += _utc_offset*60; - - struct tm *ttp = gmtime_r(&utc_time_sec, tt); - return (ttp != NULL); - } else { - return false; - } -} - -int create_log_dir() -{ - /* create dir on sdcard if needed */ - uint16_t dir_number = 1; // start with dir sess001 - int mkdir_ret; - - struct tm tt; - bool time_ok = get_log_time_tt(&tt, true); - - if (log_name_timestamp && time_ok) { - int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); - if (n >= sizeof(log_dir)) { - PX4_ERR("log path too long"); - return -1; - } - strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); - mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - - if ((mkdir_ret != OK) && (errno != EEXIST)) { - warn("failed creating new dir: %s", log_dir); - return -1; - } - - } else { - /* Look for the next dir that does not exist. - * However, if we've already crated a sessXXX folder in this session - * let's re-use it. */ - while (dir_number <= MAX_NO_LOGFOLDER && !sess_folder_created) { - /* format log dir: e.g. /fs/microsd/sess001 */ - int n = snprintf(log_dir, sizeof(log_dir), "%s/sess%03u", log_root, dir_number); - if (n >= sizeof(log_dir)) { - PX4_ERR("log path too long"); - return -1; - } - - mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - - if (mkdir_ret == 0) { - sess_folder_created = true; - break; - - } else if (errno != EEXIST) { - warn("failed creating new dir: %s", log_dir); - return -1; - } - - /* dir exists already */ - dir_number++; - } - - if (dir_number >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - PX4_WARN("all %d possible dirs exist already", MAX_NO_LOGFOLDER); - return -1; - } - } - - /* print logging path, important to find log file later */ - mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] %s", log_dir); - - return 0; -} - -int open_log_file() -{ - /* string to hold the path to the log */ - char log_file_name[64] = ""; - char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; - - struct tm tt; - bool time_ok = get_log_time_tt(&tt, false); - - /* start logging if we have a valid time and the time is not in the past */ - if (log_name_timestamp && time_ok) { - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt); - snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - - } else { - uint16_t file_number = 1; // start with file log001 - - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.px4log */ - snprintf(log_file_name, sizeof(log_file_name), "log%03u.px4log", file_number); - snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - - if (!file_exist(log_file_path)) { - break; - } - - file_number++; - } - - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(&mavlink_log_pub, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE); - return -1; - } - } - -#ifdef __PX4_NUTTX - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); -#else - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, PX4_O_MODE_666); -#endif - - if (fd < 0) { - mavlink_log_critical(&mavlink_log_pub, "[blackbox] failed: %s", log_file_name); - - } else { - mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] recording: %s", log_file_name); - } - - return fd; -} - -int open_perf_file(const char* str) -{ - /* string to hold the path to the log */ - char log_file_name[64] = ""; - char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; - - struct tm tt; - bool time_ok = get_log_time_tt(&tt, false); - - if (log_name_timestamp && time_ok) { - strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); - if (snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name)) { - return -1; - } - - } else { - unsigned file_number = 1; // start with file log001 - - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.txt */ - snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number); - if (snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name) >= sizeof(log_file_path)) { - return -1; - } - - if (!file_exist(log_file_path)) { - break; - } - - file_number++; - } - - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(&mavlink_log_pub, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE); - return -1; - } - } - -#ifdef __PX4_NUTTX - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); -#else - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0666); -#endif - - if (fd < 0) { - mavlink_log_critical(&mavlink_log_pub, "[blackbox] failed: %s", log_file_name); - - } - - return fd; -} - -static void *logwriter_thread(void *arg) -{ - /* set name */ - px4_prctl(PR_SET_NAME, "sdlog2_writer", 0); - - int log_fd = open_log_file(); - - if (log_fd < 0) { - return NULL; - } - - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - - /* write log messages formats, version and parameters */ - log_bytes_written += write_formats(log_fd); - - log_bytes_written += write_version(log_fd); - - log_bytes_written += write_parameters(log_fd); - - fsync(log_fd); - - int poll_count = 0; - - void *read_ptr; - - int n = 0; - - bool should_wait = false; - - bool is_part = false; - - while (true) { - /* make sure threads are synchronized */ - pthread_mutex_lock(&logbuffer_mutex); - - /* update read pointer if needed */ - if (n > 0) { - logbuffer_mark_read(&lb, n); - } - - /* only wait if no data is available to process */ - if (should_wait && !logwriter_should_exit) { - /* blocking wait for new data at this line */ - pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex); - } - - /* only get pointer to thread-safe data, do heavy I/O a few lines down */ - int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); - - /* continue */ - pthread_mutex_unlock(&logbuffer_mutex); - - if (available > 0) { - - /* do heavy IO here */ - if (available > MAX_WRITE_CHUNK) { - n = MAX_WRITE_CHUNK; - - } else { - n = available; - } - - perf_begin(perf_write); - n = write(log_fd, read_ptr, n); - perf_end(perf_write); - - should_wait = (n == available) && !is_part; - - if (n < 0) { - main_thread_should_exit = true; - warn("error writing log file"); - break; - } - - if (n > 0) { - log_bytes_written += n; - } - - } else { - n = 0; - - /* exit only with empty buffer */ - if (main_thread_should_exit || logwriter_should_exit) { - break; - } - - should_wait = true; - } - - if (++poll_count == 10) { - fsync(log_fd); - poll_count = 0; - - } - - if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) { - /* check if space is available, if not stop everything */ - if (check_free_space() != OK) { - logwriter_should_exit = true; - main_thread_should_exit = true; - } - last_checked_bytes_written = log_bytes_written; - } - } - - fsync(log_fd); - close(log_fd); - - return NULL; -} - -void sdlog2_start_log() -{ - if (logging_enabled) { - return; - } - - /* create log dir if needed */ - if (create_log_dir() != 0) { - mavlink_log_critical(&mavlink_log_pub, "[blackbox] error creating log dir"); - return; - } - - /* initialize statistics counter */ - log_bytes_written = 0; - start_time = hrt_absolute_time(); - log_msgs_written = 0; - log_msgs_skipped = 0; - - /* initialize log buffer emptying thread */ - pthread_attr_init(&logwriter_attr); - -#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) - struct sched_param param; - (void)pthread_attr_getschedparam(&logwriter_attr, ¶m); - /* low priority, as this is expensive disk I/O. */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 5; - if (pthread_attr_setschedparam(&logwriter_attr, ¶m)) { - PX4_WARN("sdlog2: failed setting sched params"); - } -#endif - - pthread_attr_setstacksize(&logwriter_attr, PX4_STACK_ADJUSTED(2048)); - - logwriter_should_exit = false; - - /* allocate write performance counter */ - perf_write = perf_alloc(PC_ELAPSED, "sd write"); - - /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { - PX4_WARN("error creating logwriter thread"); - } - - /* write all performance counters */ - hrt_abstime curr_time = hrt_absolute_time(); - struct print_load_s load; - int perf_fd = open_perf_file("preflight"); - init_print_load_s(curr_time, &load); - print_load(curr_time, perf_fd, &load); - dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); - perf_print_all(perf_fd); - dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n"); - usleep(500 * 1000); - print_load(hrt_absolute_time(), perf_fd, &load); - close(perf_fd); - - /* reset performance counters to get in-flight min and max values in post flight log */ - perf_reset_all(); - - logging_enabled = true; -} - -void sdlog2_stop_log() -{ - if (!logging_enabled) { - return; - } - - /* disabling the logging will trigger the skipped count to increase, - * so we take a local copy before interrupting the disk I/O. - */ - unsigned long skipped_count = log_msgs_skipped; - - logging_enabled = false; - - /* wake up write thread one last time */ - pthread_mutex_lock(&logbuffer_mutex); - logwriter_should_exit = true; - pthread_cond_signal(&logbuffer_cond); - /* unlock, now the writer thread may return */ - pthread_mutex_unlock(&logbuffer_mutex); - - /* wait for write thread to return */ - int ret; - - if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { - PX4_WARN("error joining logwriter thread: %i", ret); - } - - logwriter_pthread = 0; - pthread_attr_destroy(&logwriter_attr); - - /* write all performance counters */ - int perf_fd = open_perf_file("postflight"); - hrt_abstime curr_time = hrt_absolute_time(); - dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); - perf_print_all(perf_fd); - struct print_load_s load; - dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n"); - init_print_load_s(curr_time, &load); - print_load(curr_time, perf_fd, &load); - sleep(1); - print_load(hrt_absolute_time(), perf_fd, &load); - close(perf_fd); - - /* free log writer performance counter */ - perf_free(perf_write); - - /* reset the logbuffer */ - logbuffer_reset(&lb); - - mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] stopped (%lu drops)", skipped_count); - - sdlog2_status(); -} - -int write_formats(int fd) -{ - /* construct message format packet */ - struct { - LOG_PACKET_HEADER; - struct log_format_s body; - } log_msg_format = { - LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), - }; - - int written = 0; - - /* fill message format packet for each format and write it */ - for (unsigned i = 0; i < log_formats_num; i++) { - log_msg_format.body = log_formats[i]; - written += write(fd, &log_msg_format, sizeof(log_msg_format)); - } - - return written; -} - -int write_version(int fd) -{ - /* construct version message */ - struct { - LOG_PACKET_HEADER; - struct log_VER_s body; - } log_msg_VER = { - LOG_PACKET_HEADER_INIT(LOG_VER_MSG), - }; - - /* fill version message and write it */ - strncpy(log_msg_VER.body.fw_git, px4_firmware_version_string(), sizeof(log_msg_VER.body.fw_git)-1); - log_msg_VER.body.fw_git[sizeof(log_msg_VER.body.fw_git)-1] = '\0'; - strncpy(log_msg_VER.body.arch, px4_board_name(), sizeof(log_msg_VER.body.arch)-1); - log_msg_VER.body.arch[sizeof(log_msg_VER.body.arch) - 1] = '\0'; - return write(fd, &log_msg_VER, sizeof(log_msg_VER)); -} - -int write_parameters(int fd) -{ - /* construct parameter message */ - struct { - LOG_PACKET_HEADER; - struct log_PARM_s body; - } log_msg_PARM = { - LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), - }; - - int written = 0; - param_t params_cnt = param_count(); - - for (param_t param = 0; param < params_cnt; param++) { - /* fill parameter message and write it */ - strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); - log_msg_PARM.body.name[sizeof(log_msg_PARM.body.name) - 1] = '\0'; - float value = NAN; - - switch (param_type(param)) { - case PARAM_TYPE_INT32: { - int32_t i; - param_get(param, &i); - value = i; // cast integer to float - break; - } - - case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; - - default: - break; - } - - log_msg_PARM.body.value = value; - written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); - } - - return written; -} - -bool copy_if_updated(orb_id_t topic, int *handle, void *buffer) -{ - return copy_if_updated_multi(topic, 0, handle, buffer); -} - -bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer) -{ - bool updated = false; - - if (*handle < 0) { - if (OK == orb_exists(topic, multi_instance)) - { - *handle = orb_subscribe_multi(topic, multi_instance); - /* copy first data */ - if (*handle >= 0) { - - /* but only if it has really been updated */ - orb_check(*handle, &updated); - - if (updated) { - orb_copy(topic, *handle, buffer); - } - } - } - } else { - orb_check(*handle, &updated); - - if (updated) { - orb_copy(topic, *handle, buffer); - } - } - - return updated; -} - -int sdlog2_thread_main(int argc, char *argv[]) -{ - /* default log rate: 50 Hz */ - int32_t log_rate = 50; - int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; - logging_enabled = false; - /* enable logging on start (-e option) */ - bool log_on_start = false; - /* enable logging when armed (-a option) */ - bool log_when_armed = false; - log_name_timestamp = false; - - flag_system_armed = false; - -#ifdef __PX4_NUTTX - /* the NuttX optarg handler does not - * ignore argv[0] like the POSIX handler - * does, nor does it deal with non-flag - * verbs well. So we Remove the application - * name and the verb. - */ - argc -= 2; - argv += 2; -#endif - - int ch; - - /* don't exit from getopt loop to leave getopt global variables in consistent state, - * set error flag instead */ - bool err_flag = false; - - int myoptind = 1; - const char *myoptarg = NULL; - while ((ch = px4_getopt(argc, argv, "r:b:eatx", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'r': { - unsigned long r = strtoul(myoptarg, NULL, 10); - - if (r <= 0) { - r = 1; - } - - log_rate = r; - } - break; - - case 'b': { - unsigned long s = strtoul(myoptarg, NULL, 10); - - if (s < 1) { - s = 1; - } - - log_buffer_size = 1024 * s; - } - break; - - case 'e': - log_on_start = true; - log_when_armed = true; - break; - - case 'a': - log_when_armed = true; - break; - - case 't': - log_name_timestamp = true; - break; - - case 'x': - _extended_logging = true; - break; - - case '?': - if (optopt == 'c') { - PX4_WARN("option -%c requires an argument", optopt); - - } else if (isprint(optopt)) { - PX4_WARN("unknown option `-%c'", optopt); - - } else { - PX4_WARN("unknown option character `\\x%x'", optopt); - } - err_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - err_flag = true; - break; - } - } - - if (err_flag) { - sdlog2_usage(NULL); - } - - gps_time_sec = 0; - - /* interpret logging params */ - int32_t param_log_rate = -1; - param_t log_rate_ph = param_find("SDLOG_RATE"); - - if (log_rate_ph != PARAM_INVALID) { - param_get(log_rate_ph, ¶m_log_rate); - - if (param_log_rate > 0) { - - /* we can't do more than ~ 500 Hz, even with a massive buffer */ - if (param_log_rate > 250) { - param_log_rate = 250; - } - - } else if (param_log_rate == 0) { - /* we need at minimum 10 Hz to be able to see anything */ - param_log_rate = 10; - } - } - - // if parameter was provided use it, if not use command line argument - log_rate = param_log_rate > -1 ? param_log_rate : log_rate; - - param_t log_ext_ph = param_find("SDLOG_EXT"); - - if (log_ext_ph != PARAM_INVALID) { - - int32_t param_log_extended; - param_get(log_ext_ph, ¶m_log_extended); - - if (param_log_extended > 0) { - _extended_logging = true; - } else if (param_log_extended == 0) { - _extended_logging = false; - } - /* any other value means to ignore the parameter, so no else case */ - - } - - param_t log_gpstime_ph = param_find("SDLOG_GPSTIME"); - - if (log_gpstime_ph != PARAM_INVALID) { - - int32_t param_log_gpstime; - param_get(log_gpstime_ph, ¶m_log_gpstime); - - if (param_log_gpstime > 0) { - _gpstime_only = true; - } else if (param_log_gpstime == 0) { - _gpstime_only = false; - } - /* any other value means to ignore the parameter, so no else case */ - - } - - param_t log_utc_offset = param_find("SDLOG_UTC_OFFSET"); - - if ( log_utc_offset != PARAM_INVALID ) { - int32_t param_utc_offset; - param_get(log_utc_offset, ¶m_utc_offset); - _utc_offset = param_utc_offset; - } - - if (check_free_space() != OK) { - return 1; - } - - - /* create log root dir */ - int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); - - if (mkdir_ret != 0 && errno != EEXIST) { - warn("ERR: failed creating log dir: %s", log_root); - return 1; - } - - /* initialize log buffer with specified size */ - PX4_DEBUG("log buffer size: %i bytes", log_buffer_size); - - if (OK != logbuffer_init(&lb, log_buffer_size)) { - PX4_WARN("can't allocate log buffer, exiting"); - return 1; - } - - struct vehicle_status_s buf_status; - memset(&buf_status, 0, sizeof(buf_status)); - - struct vehicle_gps_position_s buf_gps_pos; - memset(&buf_gps_pos, 0, sizeof(buf_gps_pos)); - - struct vehicle_command_s buf_cmd; - memset(&buf_cmd, 0, sizeof(buf_cmd)); - - struct commander_state_s buf_commander_state; - memset(&buf_commander_state, 0, sizeof(buf_commander_state)); - - /* There are different log types possible on different platforms. */ - enum { - LOG_TYPE_NORMAL, - LOG_TYPE_ALL - } log_type; - - log_type = LOG_TYPE_NORMAL; - - /* warning! using union here to save memory, elements should be used separately! */ - union { - struct vehicle_command_s cmd; - struct sensor_combined_s sensor; - struct vehicle_attitude_s att; - struct vehicle_attitude_setpoint_s att_sp; - struct vehicle_rates_setpoint_s rates_sp; - struct actuator_outputs_s act_outputs; - struct actuator_controls_s act_controls; - struct actuator_controls_s act_controls1; - struct vehicle_local_position_s local_pos; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_global_position_s global_pos; - struct position_setpoint_triplet_s triplet; - struct att_pos_mocap_s att_pos_mocap; - struct vehicle_local_position_s vision_pos; - struct vehicle_attitude_s vision_att; - struct optical_flow_s flow; - struct rc_channels_s rc; - struct differential_pressure_s diff_pres; - struct airspeed_s airspeed; - struct esc_status_s esc; - struct battery_status_s battery; - struct telemetry_status_s telemetry; - struct distance_sensor_s distance_sensor; - struct estimator_status_s estimator_status; - struct tecs_status_s tecs_status; - struct system_power_s system_power; - struct servorail_status_s servorail_status; - struct satellite_info_s sat_info; - struct wind_estimate_s wind_estimate; - struct vtol_vehicle_status_s vtol_status; - struct rate_ctrl_status_s rate_ctrl_status; - struct ekf2_innovations_s innovations; - struct camera_trigger_s camera_trigger; - struct vehicle_land_detected_s land_detected; - struct cpuload_s cpuload; - struct vehicle_gps_position_s dual_gps_pos; - struct task_stack_info_s task_stack_info; - } buf; - - memset(&buf, 0, sizeof(buf)); - - /* log message buffer: header + body */ -#pragma pack(push, 1) - struct { - LOG_PACKET_HEADER; - union { - struct log_TIME_s log_TIME; - struct log_ATT_s log_ATT; - struct log_ATSP_s log_ATSP; - struct log_IMU_s log_IMU; - struct log_SENS_s log_SENS; - struct log_LPOS_s log_LPOS; - struct log_LPSP_s log_LPSP; - struct log_GPS_s log_GPS; - struct log_ATTC_s log_ATTC; - struct log_STAT_s log_STAT; - struct log_VTOL_s log_VTOL; - struct log_RC_s log_RC; - struct log_OUT_s log_OUT; - struct log_AIRS_s log_AIRS; - struct log_ARSP_s log_ARSP; - struct log_FLOW_s log_FLOW; - struct log_GPOS_s log_GPOS; - struct log_GPSP_s log_GPSP; - struct log_ESC_s log_ESC; - struct log_GVSP_s log_GVSP; - struct log_BATT_s log_BATT; - struct log_DIST_s log_DIST; - struct log_TEL_s log_TEL; - struct log_EST0_s log_EST0; - struct log_EST1_s log_EST1; - struct log_EST2_s log_EST2; - struct log_EST3_s log_EST3; - struct log_PWR_s log_PWR; - struct log_MOCP_s log_MOCP; - struct log_VISN_s log_VISN; - struct log_GS0A_s log_GS0A; - struct log_GS0B_s log_GS0B; - struct log_GS1A_s log_GS1A; - struct log_GS1B_s log_GS1B; - struct log_TECS_s log_TECS; - struct log_WIND_s log_WIND; - struct log_ENCD_s log_ENCD; - struct log_TSYN_s log_TSYN; - struct log_MACS_s log_MACS; - struct log_CTS_s log_CTS; - struct log_EST4_s log_INO1; - struct log_EST5_s log_INO2; - struct log_CAMT_s log_CAMT; - struct log_EST6_s log_INO3; - struct log_LAND_s log_LAND; - struct log_LOAD_s log_LOAD; - struct log_DPRS_s log_DPRS; - struct log_STCK_s log_STCK; - } body; - } log_msg = { - LOG_PACKET_HEADER_INIT(0) - }; -#pragma pack(pop) - memset(&log_msg.body, 0, sizeof(log_msg.body)); - - struct { - int cmd_sub; - int status_sub; - int vtol_status_sub; - int sensor_sub; - int att_sub; - int att_sp_sub; - int rates_sp_sub; - int act_outputs_sub; - int act_outputs_1_sub; - int act_controls_sub; - int act_controls_1_sub; - int local_pos_sub; - int local_pos_sp_sub; - int global_pos_sub; - int triplet_sub; - int gps_pos_sub[2]; - int sat_info_sub; - int att_pos_mocap_sub; - int vision_pos_sub; - int vision_att_sub; - int flow_sub; - int rc_sub; - int airspeed_sub; - int esc_sub; - int battery_sub; - int telemetry_subs[ORB_MULTI_MAX_INSTANCES]; - int distance_sensor_sub; - int estimator_status_sub; - int tecs_status_sub; - int system_power_sub; - int servorail_status_sub; - int wind_sub; - int tsync_sub; - int rate_ctrl_status_sub; - int innov_sub; - int cam_trig_sub; - int land_detected_sub; - int commander_state_sub; - int cpuload_sub; - int diff_pres_sub; - int task_stack_info_sub; - } subs; - - subs.cmd_sub = -1; - subs.status_sub = -1; - subs.vtol_status_sub = -1; - subs.gps_pos_sub[0] = -1; - subs.gps_pos_sub[1] = -1; - subs.sensor_sub = -1; - subs.att_sub = -1; - subs.att_sp_sub = -1; - subs.rates_sp_sub = -1; - subs.act_outputs_sub = -1; - subs.act_outputs_1_sub = -1; - subs.act_controls_sub = -1; - subs.act_controls_1_sub = -1; - subs.local_pos_sub = -1; - subs.local_pos_sp_sub = -1; - subs.global_pos_sub = -1; - subs.triplet_sub = -1; - subs.att_pos_mocap_sub = -1; - subs.vision_pos_sub = -1; - subs.vision_att_sub = -1; - subs.flow_sub = -1; - subs.rc_sub = -1; - subs.airspeed_sub = -1; - subs.esc_sub = -1; - subs.battery_sub = -1; - subs.distance_sensor_sub = -1; - subs.estimator_status_sub = -1; - subs.tecs_status_sub = -1; - subs.system_power_sub = -1; - subs.servorail_status_sub = -1; - subs.wind_sub = -1; - subs.tsync_sub = -1; - subs.rate_ctrl_status_sub = -1; - subs.innov_sub = -1; - subs.cam_trig_sub = -1; - subs.land_detected_sub = -1; - subs.commander_state_sub = -1; - subs.cpuload_sub = -1; - subs.diff_pres_sub = -1; - subs.task_stack_info_sub = -1; - - /* add new topics HERE */ - - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - subs.telemetry_subs[i] = -1; - } - - subs.sat_info_sub = -1; - - /* initialize thread synchronization */ - pthread_mutex_init(&logbuffer_mutex, NULL); - pthread_cond_init(&logbuffer_cond, NULL); - - /* track changes in sensor_combined topic */ - hrt_abstime gyro_timestamp = 0; - hrt_abstime accelerometer_timestamp = 0; - //hrt_abstime magnetometer_timestamp = 0; - //hrt_abstime barometer_timestamp = 0; - - /* initialize calculated mean SNR */ - float snr_mean = 0.0f; - - /* enable logging on start if needed */ - if (log_on_start) { - /* check GPS topic to get GPS time */ - if (log_name_timestamp) { - if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos)) { - gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; - } - } - - sdlog2_start_log(); - } - - /* running, report */ - thread_running = true; - - // wakeup source - px4_pollfd_struct_t fds[1]; - unsigned px4_pollfd_len = 0; - - int poll_counter = 0; - - int poll_to_logging_factor = 1; - - switch (log_type) { - case LOG_TYPE_ALL: - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[0].fd = subs.sensor_sub; - fds[0].events = POLLIN; - - px4_pollfd_len = 1; - - poll_to_logging_factor = 1; - - break; - - case LOG_TYPE_NORMAL: - - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[0].fd = subs.sensor_sub; - fds[0].events = POLLIN; - - px4_pollfd_len = 1; - - // TODO Remove hardcoded rate! - poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate); - - break; - } - - if (poll_to_logging_factor < 1) { - poll_to_logging_factor = 1; - } - - - while (!main_thread_should_exit) { - - /* Check below's topics first even if logging is not enabled. - * We need to do this because should only poll further below if we're - * actually going to orb_copy the data after the poll. */ - - /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ - if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf_cmd)) { - handle_command(&buf_cmd); - } - - /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ - bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status); - - if (status_updated) { - if (log_when_armed) { - handle_status(&buf_status); - } - } - - /* --- GPS POSITION - LOG MANAGEMENT --- */ - bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos); - - if (gps_pos_updated && log_name_timestamp) { - gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; - has_gps_3d_fix = buf_gps_pos.fix_type >= 3; - } - - if (!logging_enabled) { - usleep(50000); - continue; - } - - // wait for up to 100ms for data - int pret = px4_poll(&fds[0], px4_pollfd_len, 100); - - // timed out - periodic check for _task_should_exit - if (pret == 0) { - continue; - } - - // this is undesirable but not much we can do - might want to flag unhappy status - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); - continue; - } - - if ((poll_counter+1) >= poll_to_logging_factor) { - poll_counter = 0; - } else { - - /* In this case, we still need to do orb_copy, otherwise we'll stall. */ - switch (log_type) { - case LOG_TYPE_ALL: - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - } - break; - - case LOG_TYPE_NORMAL: - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - } - break; - } - poll_counter++; - continue; - } - - /* write time stamp message */ - log_msg.msg_type = LOG_TIME_MSG; - log_msg.body.log_TIME.t = hrt_absolute_time(); - LOGBUFFER_WRITE_AND_COUNT(TIME); - - /* --- COMMANDER INTERNAL STATE --- */ - copy_if_updated(ORB_ID(commander_state), &subs.commander_state_sub, - &buf_commander_state); - - /* --- VEHICLE STATUS --- */ - if (status_updated) { - log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = buf_commander_state.main_state; - log_msg.body.log_STAT.nav_state = buf_status.nav_state; - log_msg.body.log_STAT.arming_state = buf_status.arming_state; - log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; - log_msg.body.log_STAT.is_rot_wing = (uint8_t)buf_status.is_rotary_wing; - LOGBUFFER_WRITE_AND_COUNT(STAT); - } - - if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_NORMAL) { - - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - - bool write_IMU = false; - bool write_SENS = false; - - if (buf.sensor.timestamp != gyro_timestamp) { - gyro_timestamp = buf.sensor.timestamp; - write_IMU = true; - } - - if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) { - accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative; - write_IMU = true; - } - -// if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) { -// magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative; -// write_IMU = true; -// } - -// if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) { -// barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative; -// write_SENS = true; -// } - - if (write_IMU) { - log_msg.msg_type = LOG_IMU_MSG; - - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; -// log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; -// log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; -// log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - log_msg.body.log_IMU.temp_gyro = 0; - log_msg.body.log_IMU.temp_acc = 0; - log_msg.body.log_IMU.temp_mag = 0; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (write_SENS) { -// log_msg.msg_type = LOG_SENS_MSG; -// -// log_msg.body.log_SENS.baro_pres = 0; -// log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; -// log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; -// LOGBUFFER_WRITE_AND_COUNT(SENS); - } - } - - /* --- VTOL VEHICLE STATUS --- */ - if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) { - log_msg.msg_type = LOG_VTOL_MSG; - log_msg.body.log_VTOL.rw_mode = buf.vtol_status.vtol_in_rw_mode; - log_msg.body.log_VTOL.trans_mode = buf.vtol_status.vtol_in_trans_mode; - log_msg.body.log_VTOL.failsafe_mode = buf.vtol_status.vtol_transition_failsafe; - LOGBUFFER_WRITE_AND_COUNT(VTOL); - } - - /* --- GPS POSITION - UNIT #1 --- */ - if (gps_pos_updated) { - - log_msg.msg_type = LOG_GPS_MSG; - log_msg.body.log_GPS.gps_time = buf_gps_pos.time_utc_usec; - log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf_gps_pos.eph; - log_msg.body.log_GPS.epv = buf_gps_pos.epv; - log_msg.body.log_GPS.lat = buf_gps_pos.lat; - log_msg.body.log_GPS.lon = buf_gps_pos.lon; - log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; - log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s; - log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; - log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; - log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; - log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used; - log_msg.body.log_GPS.snr_mean = snr_mean; - log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; - log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; - LOGBUFFER_WRITE_AND_COUNT(GPS); - } - - /* --- GPS POSITION - UNIT #2 --- */ - if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub[1], &buf.dual_gps_pos)) { - log_msg.msg_type = LOG_DGPS_MSG; - log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec; - log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph; - log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv; - log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat; - log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon; - log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f; - log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s; - log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s; - log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s; - log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad; - log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used; - log_msg.body.log_GPS.snr_mean = snr_mean; - log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms; - log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator; - LOGBUFFER_WRITE_AND_COUNT(GPS); - } - - /* --- SATELLITE INFO - UNIT #1 --- */ - if (_extended_logging) { - - if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) { - - /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned sat_info_count = SDLOG_MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); - unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); - - log_msg.msg_type = LOG_GS0A_MSG; - memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); - snr_mean = 0.0f; - - /* fill set A and calculate mean SNR */ - for (unsigned i = 0; i < sat_info_count; i++) { - - snr_mean += buf.sat_info.snr[i]; - - int satindex = buf.sat_info.svid[i] - 1; - - /* handles index exceeding and wraps to to arithmetic errors */ - if ((satindex >= 0) && (satindex < (int)log_max_snr)) { - /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; - } - } - LOGBUFFER_WRITE_AND_COUNT(GS0A); - snr_mean /= sat_info_count; - - log_msg.msg_type = LOG_GS0B_MSG; - memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); - - /* fill set B */ - for (unsigned i = 0; i < sat_info_count; i++) { - - /* get second bank of satellites, thus deduct bank size from index */ - int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; - - /* handles index exceeding and wraps to to arithmetic errors */ - if ((satindex >= 0) && (satindex < (int)log_max_snr)) { - /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; - } - } - LOGBUFFER_WRITE_AND_COUNT(GS0B); - } - } - - /* --- ATTITUDE SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) { - log_msg.msg_type = LOG_ATSP_MSG; - log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; - log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; - log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; - log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; - log_msg.body.log_ATSP.q_w = buf.att_sp.q_d[0]; - log_msg.body.log_ATSP.q_x = buf.att_sp.q_d[1]; - log_msg.body.log_ATSP.q_y = buf.att_sp.q_d[2]; - log_msg.body.log_ATSP.q_z = buf.att_sp.q_d[3]; - LOGBUFFER_WRITE_AND_COUNT(ATSP); - } - - /* --- RATES SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), &subs.rates_sp_sub, &buf.rates_sp)) { - log_msg.msg_type = LOG_ARSP_MSG; - log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; - log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; - log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; - LOGBUFFER_WRITE_AND_COUNT(ARSP); - } - - /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated_multi(ORB_ID(actuator_outputs), 0, &subs.act_outputs_sub, &buf.act_outputs)) { - log_msg.msg_type = LOG_OUT0_MSG; - memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output)); - LOGBUFFER_WRITE_AND_COUNT(OUT); - } - - if (copy_if_updated_multi(ORB_ID(actuator_outputs), 1, &subs.act_outputs_1_sub, &buf.act_outputs)) { - log_msg.msg_type = LOG_OUT1_MSG; - memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output)); - LOGBUFFER_WRITE_AND_COUNT(OUT); - } - - /* --- ACTUATOR CONTROL --- */ - if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &subs.act_controls_sub, &buf.act_controls)) { - log_msg.msg_type = LOG_ATTC_MSG; - log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; - log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; - log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; - log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; - LOGBUFFER_WRITE_AND_COUNT(ATTC); - } - - /* --- ACTUATOR CONTROL FW VTOL --- */ - if(copy_if_updated(ORB_ID(actuator_controls_1), &subs.act_controls_1_sub,&buf.act_controls)) { - log_msg.msg_type = LOG_ATC1_MSG; - log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; - log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; - log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; - log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; - LOGBUFFER_WRITE_AND_COUNT(ATTC); - } - - /* --- LOCAL POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_local_position), &subs.local_pos_sub, &buf.local_pos)) { - log_msg.msg_type = LOG_LPOS_MSG; - log_msg.body.log_LPOS.x = buf.local_pos.x; - log_msg.body.log_LPOS.y = buf.local_pos.y; - log_msg.body.log_LPOS.z = buf.local_pos.z; - log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom; - log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate; - log_msg.body.log_LPOS.vx = buf.local_pos.vx; - log_msg.body.log_LPOS.vy = buf.local_pos.vy; - log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; - log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) | - (buf.local_pos.z_valid ? 2 : 0) | - (buf.local_pos.v_xy_valid ? 4 : 0) | - (buf.local_pos.v_z_valid ? 8 : 0) | - (buf.local_pos.xy_global ? 16 : 0) | - (buf.local_pos.z_global ? 32 : 0); - log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); - log_msg.body.log_LPOS.eph = buf.local_pos.eph; - log_msg.body.log_LPOS.epv = buf.local_pos.epv; - LOGBUFFER_WRITE_AND_COUNT(LPOS); - } - - /* --- LOCAL POSITION SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), &subs.local_pos_sp_sub, &buf.local_pos_sp)) { - log_msg.msg_type = LOG_LPSP_MSG; - log_msg.body.log_LPSP.x = buf.local_pos_sp.x; - log_msg.body.log_LPSP.y = buf.local_pos_sp.y; - log_msg.body.log_LPSP.z = buf.local_pos_sp.z; - log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; - log_msg.body.log_LPSP.vx = buf.local_pos_sp.vx; - log_msg.body.log_LPSP.vy = buf.local_pos_sp.vy; - log_msg.body.log_LPSP.vz = buf.local_pos_sp.vz; - log_msg.body.log_LPSP.acc_x = buf.local_pos_sp.acc_x; - log_msg.body.log_LPSP.acc_y = buf.local_pos_sp.acc_y; - log_msg.body.log_LPSP.acc_z = buf.local_pos_sp.acc_z; - LOGBUFFER_WRITE_AND_COUNT(LPSP); - } - - /* --- GLOBAL POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_global_position), &subs.global_pos_sub, &buf.global_pos)) { - log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; - log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; - log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.eph = buf.global_pos.eph; - log_msg.body.log_GPOS.epv = buf.global_pos.epv; - if (buf.global_pos.terrain_alt_valid) { - log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt; - } else { - log_msg.body.log_GPOS.terrain_alt = -1.0f; - } - LOGBUFFER_WRITE_AND_COUNT(GPOS); - } - - /* --- BATTERY --- */ - if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) { - log_msg.msg_type = LOG_BATT_MSG; - log_msg.body.log_BATT.voltage = buf.battery.voltage_v; - log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; - log_msg.body.log_BATT.current = buf.battery.current_a; - log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a; - log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; - log_msg.body.log_BATT.remaining = buf.battery.remaining; - log_msg.body.log_BATT.scale = buf.battery.scale; - log_msg.body.log_BATT.warning = buf.battery.warning; - LOGBUFFER_WRITE_AND_COUNT(BATT); - } - - /* --- GLOBAL POSITION SETPOINT --- */ - if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) { - - if (buf.triplet.current.valid) { - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * (double)1e7); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * (double)1e7); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); - } - } - - /* --- MOCAP ATTITUDE AND POSITION --- */ - if (copy_if_updated(ORB_ID(att_pos_mocap), &subs.att_pos_mocap_sub, &buf.att_pos_mocap)) { - log_msg.msg_type = LOG_MOCP_MSG; - log_msg.body.log_MOCP.qw = buf.att_pos_mocap.q[0]; - log_msg.body.log_MOCP.qx = buf.att_pos_mocap.q[1]; - log_msg.body.log_MOCP.qy = buf.att_pos_mocap.q[2]; - log_msg.body.log_MOCP.qz = buf.att_pos_mocap.q[3]; - log_msg.body.log_MOCP.x = buf.att_pos_mocap.x; - log_msg.body.log_MOCP.y = buf.att_pos_mocap.y; - log_msg.body.log_MOCP.z = buf.att_pos_mocap.z; - LOGBUFFER_WRITE_AND_COUNT(MOCP); - } - - /* --- VISION POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vision_position), &subs.vision_pos_sub, &buf.vision_pos) || - copy_if_updated(ORB_ID(vehicle_vision_attitude), &subs.vision_att_sub, &buf.vision_att)) { - log_msg.msg_type = LOG_VISN_MSG; - log_msg.body.log_VISN.x = buf.vision_pos.x; - log_msg.body.log_VISN.y = buf.vision_pos.y; - log_msg.body.log_VISN.z = buf.vision_pos.z; - log_msg.body.log_VISN.vx = buf.vision_pos.vx; - log_msg.body.log_VISN.vy = buf.vision_pos.vy; - log_msg.body.log_VISN.vz = buf.vision_pos.vz; - log_msg.body.log_VISN.qw = buf.vision_att.q[0]; // vision_position_estimate uses [w,x,y,z] convention - log_msg.body.log_VISN.qx = buf.vision_att.q[1]; - log_msg.body.log_VISN.qy = buf.vision_att.q[2]; - log_msg.body.log_VISN.qz = buf.vision_att.q[3]; - LOGBUFFER_WRITE_AND_COUNT(VISN); - } - - /* --- FLOW --- */ - if (copy_if_updated(ORB_ID(optical_flow), &subs.flow_sub, &buf.flow)) { - log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; - log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; - log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral; - log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral; - log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral; - log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan; - log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral; - log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral; - log_msg.body.log_FLOW.quality = buf.flow.quality; - log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; - LOGBUFFER_WRITE_AND_COUNT(FLOW); - } - - /* --- RC CHANNELS --- */ - if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { - log_msg.msg_type = LOG_RC_MSG; - /* Copy only the first 12 channels of 18 */ - memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); - log_msg.body.log_RC.rssi = buf.rc.rssi; - log_msg.body.log_RC.channel_count = buf.rc.channel_count; - log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; - log_msg.body.log_RC.frame_drop = buf.rc.frame_drop_count; - LOGBUFFER_WRITE_AND_COUNT(RC); - } - - /* --- AIRSPEED --- */ - if (copy_if_updated(ORB_ID(airspeed), &subs.airspeed_sub, &buf.airspeed)) { - log_msg.msg_type = LOG_AIRS_MSG; - log_msg.body.log_AIRS.indicated_airspeed_m_s = buf.airspeed.indicated_airspeed_m_s; - log_msg.body.log_AIRS.true_airspeed_m_s = buf.airspeed.true_airspeed_m_s; - log_msg.body.log_AIRS.true_airspeed_unfiltered_m_s = buf.airspeed.true_airspeed_unfiltered_m_s; - log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius; - log_msg.body.log_AIRS.confidence = buf.airspeed.confidence; - LOGBUFFER_WRITE_AND_COUNT(AIRS); - } - - /* --- DIFFERENTIAL PRESSURE --- */ - if (copy_if_updated(ORB_ID(differential_pressure), &subs.diff_pres_sub, &buf.diff_pres)) { - log_msg.msg_type = LOG_DPRS_MSG; - log_msg.body.log_DPRS.error_count = buf.diff_pres.error_count; - log_msg.body.log_DPRS.differential_pressure_raw_pa = buf.diff_pres.differential_pressure_raw_pa; - log_msg.body.log_DPRS.differential_pressure_filtered_pa = buf.diff_pres.differential_pressure_filtered_pa; - log_msg.body.log_DPRS.temperature = buf.diff_pres.temperature; - LOGBUFFER_WRITE_AND_COUNT(DPRS); - } - - /* --- ESCs --- */ - if (copy_if_updated(ORB_ID(esc_status), &subs.esc_sub, &buf.esc)) { - for (uint8_t i = 0; i < buf.esc.esc_count; i++) { - log_msg.msg_type = LOG_ESC_MSG; - log_msg.body.log_ESC.counter = buf.esc.counter; - log_msg.body.log_ESC.esc_count = buf.esc.esc_count; - log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; - log_msg.body.log_ESC.esc_num = i; - log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; - log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; - log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; - log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; - log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; - log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; - log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; - log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; - LOGBUFFER_WRITE_AND_COUNT(ESC); - } - } - - /* --- BATTERY --- */ - if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) { - log_msg.msg_type = LOG_BATT_MSG; - log_msg.body.log_BATT.voltage = buf.battery.voltage_v; - log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; - log_msg.body.log_BATT.current = buf.battery.current_a; - log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a; - log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; - LOGBUFFER_WRITE_AND_COUNT(BATT); - } - - /* --- SYSTEM POWER RAILS --- */ - if (copy_if_updated(ORB_ID(system_power), &subs.system_power_sub, &buf.system_power)) { - log_msg.msg_type = LOG_PWR_MSG; - log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; - log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; - log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; - log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; - log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC; - log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC; - - /* copy servo rail status topic here too */ - orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); - log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v; - - LOGBUFFER_WRITE_AND_COUNT(PWR); - } - - /* --- TELEMETRY --- */ - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (copy_if_updated_multi(ORB_ID(telemetry_status), i, &subs.telemetry_subs[i], &buf.telemetry)) { - log_msg.msg_type = LOG_TEL0_MSG + i; - log_msg.body.log_TEL.rssi = buf.telemetry.rssi; - log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TEL.noise = buf.telemetry.noise; - log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TEL.fixed = buf.telemetry.fixed; - log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; - log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; - LOGBUFFER_WRITE_AND_COUNT(TEL); - } - } - - /* --- DISTANCE SENSOR --- */ - if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.id = buf.distance_sensor.id; - log_msg.body.log_DIST.type = buf.distance_sensor.type; - log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation; - log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance; - log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance; - LOGBUFFER_WRITE_AND_COUNT(DIST); - } - - /* --- ESTIMATOR STATUS --- */ - if (copy_if_updated(ORB_ID(estimator_status), &subs.estimator_status_sub, &buf.estimator_status)) { - log_msg.msg_type = LOG_EST0_MSG; - unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); - memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); - memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0); - log_msg.body.log_EST0.n_states = buf.estimator_status.n_states; - log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags; - log_msg.body.log_EST0.fault_flags = buf.estimator_status.filter_fault_flags; - log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags; - LOGBUFFER_WRITE_AND_COUNT(EST0); - - log_msg.msg_type = LOG_EST1_MSG; - unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); - memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); - memcpy(&(log_msg.body.log_EST1.s), ((char*)buf.estimator_status.states) + maxcopy0, maxcopy1); - LOGBUFFER_WRITE_AND_COUNT(EST1); - - log_msg.msg_type = LOG_EST2_MSG; - unsigned maxcopy2 = (sizeof(buf.estimator_status.covariances) < sizeof(log_msg.body.log_EST2.cov)) ? sizeof(buf.estimator_status.covariances) : sizeof(log_msg.body.log_EST2.cov); - memset(&(log_msg.body.log_EST2.cov), 0, sizeof(log_msg.body.log_EST2.cov)); - memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2); - log_msg.body.log_EST2.gps_check_fail_flags = buf.estimator_status.gps_check_fail_flags; - log_msg.body.log_EST2.control_mode_flags = buf.estimator_status.control_mode_flags; - log_msg.body.log_EST2.health_flags = buf.estimator_status.health_flags; - log_msg.body.log_EST2.innov_test_flags = buf.estimator_status.innovation_check_flags; - LOGBUFFER_WRITE_AND_COUNT(EST2); - - log_msg.msg_type = LOG_EST3_MSG; - unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov); - memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov)); - memcpy(&(log_msg.body.log_EST3.cov), ((char*)buf.estimator_status.covariances) + maxcopy2, maxcopy3); - LOGBUFFER_WRITE_AND_COUNT(EST3); - } - - /* --- EKF2 INNOVATIONS --- */ - if (copy_if_updated(ORB_ID(ekf2_innovations), &subs.innov_sub, &buf.innovations)) { - log_msg.msg_type = LOG_EST4_MSG; - memset(&(log_msg.body.log_INO1.s), 0, sizeof(log_msg.body.log_INO1.s)); - for (unsigned i = 0; i < 6; i++) { - log_msg.body.log_INO1.s[i] = buf.innovations.vel_pos_innov[i]; - log_msg.body.log_INO1.s[i + 6] = buf.innovations.vel_pos_innov_var[i]; - } - for (unsigned i = 0; i < 3; i++) { - log_msg.body.log_INO1.s[i + 12] = buf.innovations.output_tracking_error[i]; - } - LOGBUFFER_WRITE_AND_COUNT(EST4); - - log_msg.msg_type = LOG_EST5_MSG; - memset(&(log_msg.body.log_INO2.s), 0, sizeof(log_msg.body.log_INO2.s)); - for (unsigned i = 0; i < 3; i++) { - log_msg.body.log_INO2.s[i] = buf.innovations.mag_innov[i]; - log_msg.body.log_INO2.s[i + 3] = buf.innovations.mag_innov_var[i]; - } - - log_msg.body.log_INO2.s[6] = buf.innovations.heading_innov; - log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var; - log_msg.body.log_INO2.s[8] = buf.innovations.airspeed_innov; - log_msg.body.log_INO2.s[9] = buf.innovations.airspeed_innov_var; - log_msg.body.log_INO2.s[10] = buf.innovations.beta_innov; - log_msg.body.log_INO2.s[11] = buf.innovations.beta_innov_var; - LOGBUFFER_WRITE_AND_COUNT(EST5); - - log_msg.msg_type = LOG_EST6_MSG; - memset(&(log_msg.body.log_INO3.s), 0, sizeof(log_msg.body.log_INO3.s)); - for(unsigned i = 0; i < 2; i++) { - log_msg.body.log_INO3.s[i] = buf.innovations.flow_innov[i]; - log_msg.body.log_INO3.s[i + 2] = buf.innovations.flow_innov_var[i]; - } - log_msg.body.log_INO3.s[4] = buf.innovations.hagl_innov; - log_msg.body.log_INO3.s[5] = buf.innovations.hagl_innov_var; - LOGBUFFER_WRITE_AND_COUNT(EST6); - } - - /* --- TECS STATUS --- */ - if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) { - log_msg.msg_type = LOG_TECS_MSG; - log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; - log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; - log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; - log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; - log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; - log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; - log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; - log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; - log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; - log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; - log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; - log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; - log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ; - log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; - log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; - LOGBUFFER_WRITE_AND_COUNT(TECS); - } - - /* --- WIND ESTIMATE --- */ - if (copy_if_updated(ORB_ID(wind_estimate), &subs.wind_sub, &buf.wind_estimate)) { - log_msg.msg_type = LOG_WIND_MSG; - log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; - log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; - log_msg.body.log_WIND.cov_x = buf.wind_estimate.variance_north; - log_msg.body.log_WIND.cov_y = buf.wind_estimate.variance_east; - LOGBUFFER_WRITE_AND_COUNT(WIND); - } - - /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ - if (copy_if_updated(ORB_ID(rate_ctrl_status), &subs.rate_ctrl_status_sub, &buf.rate_ctrl_status)) { - log_msg.msg_type = LOG_MACS_MSG; - log_msg.body.log_MACS.roll_rate_integ = buf.rate_ctrl_status.rollspeed_integ; - log_msg.body.log_MACS.pitch_rate_integ = buf.rate_ctrl_status.pitchspeed_integ; - log_msg.body.log_MACS.yaw_rate_integ = buf.rate_ctrl_status.yawspeed_integ; - LOGBUFFER_WRITE_AND_COUNT(MACS); - } - } - - /* --- ATTITUDE --- */ - if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) { - log_msg.msg_type = LOG_ATT_MSG; - float q0 = buf.att.q[0]; - float q1 = buf.att.q[1]; - float q2 = buf.att.q[2]; - float q3 = buf.att.q[3]; - log_msg.body.log_ATT.q_w = q0; - log_msg.body.log_ATT.q_x = q1; - log_msg.body.log_ATT.q_y = q2; - log_msg.body.log_ATT.q_z = q3; - log_msg.body.log_ATT.roll = atan2f(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)); - log_msg.body.log_ATT.pitch = asinf(2*(q0*q2 - q3*q1)); - log_msg.body.log_ATT.yaw = atan2f(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)); - log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; - log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; - log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; - LOGBUFFER_WRITE_AND_COUNT(ATT); - } - - /* --- CAMERA TRIGGER --- */ - if (copy_if_updated(ORB_ID(camera_trigger), &subs.cam_trig_sub, &buf.camera_trigger)) { - log_msg.msg_type = LOG_CAMT_MSG; - log_msg.body.log_CAMT.timestamp = buf.camera_trigger.timestamp; - log_msg.body.log_CAMT.seq = buf.camera_trigger.seq; - LOGBUFFER_WRITE_AND_COUNT(CAMT); - } - - /* --- LAND DETECTED --- */ - if (copy_if_updated(ORB_ID(vehicle_land_detected), &subs.land_detected_sub, &buf.land_detected)) { - log_msg.msg_type = LOG_LAND_MSG; - log_msg.body.log_LAND.landed = buf.land_detected.landed; - LOGBUFFER_WRITE_AND_COUNT(LAND); - } - - /* --- LOAD --- */ - if (copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload)) { - log_msg.msg_type = LOG_LOAD_MSG; - log_msg.body.log_LOAD.cpu_load = buf.cpuload.load; - LOGBUFFER_WRITE_AND_COUNT(LOAD); - } - - /* --- STACK --- */ - if (copy_if_updated(ORB_ID(task_stack_info), &subs.task_stack_info_sub, &buf.task_stack_info)) { - log_msg.msg_type = LOG_STCK_MSG; - log_msg.body.log_STCK.stack_free = buf.task_stack_info.stack_free; - strncpy(log_msg.body.log_STCK.task_name, (char*)buf.task_stack_info.task_name, - sizeof(log_msg.body.log_STCK.task_name)); - LOGBUFFER_WRITE_AND_COUNT(STCK); - } - - pthread_mutex_lock(&logbuffer_mutex); - - /* signal the other thread new data, but not yet unlock */ - if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { - /* only request write if several packets can be written at once */ - pthread_cond_signal(&logbuffer_cond); - } - - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&logbuffer_mutex); - } - - if (logging_enabled) { - sdlog2_stop_log(); - } - - pthread_mutex_destroy(&logbuffer_mutex); - pthread_cond_destroy(&logbuffer_cond); - - /* free log buffer */ - logbuffer_free(&lb); - - thread_running = false; - - return 0; -} - -void sdlog2_status() -{ - PX4_WARN("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - PX4_WARN("time: gps: %u seconds", (unsigned)gps_time_sec); - if (!logging_enabled) { - PX4_WARN("not logging"); - } else { - - float kibibytes = log_bytes_written / 1024.0f; - float mebibytes = kibibytes / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - - PX4_WARN("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(&mavlink_log_pub, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); - } -} - -/** - * @return true if file exists - */ -bool file_exist(const char *filename) -{ - struct stat buffer; - return stat(filename, &buffer) == 0; -} - -int check_free_space() -{ - /* use statfs to determine the number of blocks left */ - FAR struct statfs statfs_buf; - if (statfs(mountpoint, &statfs_buf) != OK) { - mavlink_log_critical(&mavlink_log_pub, "[blackbox] no microSD card, disabling logging"); - return PX4_ERROR; - } - - /* use a threshold of 50 MiB */ - if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { - mavlink_log_critical(&mavlink_log_pub, "[blackbox] no space on MicroSD: %u MiB", - (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); - /* we do not need a flag to remember that we sent this warning because we will exit anyway */ - return PX4_ERROR; - - /* use a threshold of 100 MiB to send a warning */ - } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { - mavlink_log_critical(&mavlink_log_pub, "[blackbox] space on MicroSD low: %u MiB", - (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); - /* we don't want to flood the user with warnings */ - space_warning_sent = true; - } - - return PX4_OK; -} - -void handle_command(struct vehicle_command_s *cmd) -{ - int param; - - /* request to set different system mode */ - switch (cmd->command) { - - case VEHICLE_COMMAND_VEHICLE_CMD_PREFLIGHT_STORAGE: - param = (int)(cmd->param3 + 0.5f); - - if (param == 1) { - sdlog2_start_log(); - - } else if (param == 2) { - sdlog2_stop_log(); - } else { - // Silently ignore non-matching command values, as they could be for params. - } - - break; - - default: - /* silently ignore */ - break; - } -} - -void handle_status(struct vehicle_status_s *status) -{ - // TODO use flag from actuator_armed here? - bool armed = status->arming_state == VEHICLE_STATUS_ARMING_STATE_ARMED; - - if (armed != flag_system_armed) { - flag_system_armed = armed; - - if (flag_system_armed) { - sdlog2_start_log(); - - } else { - sdlog2_stop_log(); - } - } -} diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h deleted file mode 100644 index 5fd6bf178b..0000000000 --- a/src/modules/sdlog2/sdlog2_format.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 - */ - -/* -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_ */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h deleted file mode 100644 index 60b58d4fc1..0000000000 --- a/src/modules/sdlog2/sdlog2_messages.h +++ /dev/null @@ -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 - * @author Lorenz Meier - * @author Roman Bapst - */ - -#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_ */ diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 01ed58b358..e225589d6a 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -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 *