aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/logging/logconv.m266
-rw-r--r--apps/drivers/hmc5883/hmc5883.cpp6
-rw-r--r--apps/px4io/safety.c33
3 files changed, 90 insertions, 215 deletions
diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index 48399f1eb..bf70d6d0f 100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -1,6 +1,14 @@
+% This Matlab Script can be used to import the binary logged values of the
+% PX4FMU into data that can be plotted and analyzed.
+
+% Clear everything
+clc
clear all
close all
+% Set the path to your sysvector.bin file here
+filePath = 'sysvector.bin';
+
%%%%%%%%%%%%%%%%%%%%%%%
% SYSTEM VECTOR
%
@@ -21,224 +29,72 @@ close all
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
% float attitude[3]; //pitch, roll, yaw [rad]
% float rotMatrix[9]; //unitvectors
+% float actuator_control[4]; //unitvector
+% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+
+% Definition of the logged values
+logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
+logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{11} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{12} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{13} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
+logFormat{14} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{15} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{16} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{17} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{18} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
+
+
+% First get length of one line
+columns = length(logFormat);
+lineLength = 0;
+
+for i=1:columns
+ lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;
+end
-%myPath = '..\LOG30102012\session0002\'; %set relative path here
-myPath = '.\';
-myFile = 'sysvector.bin';
-filePath = strcat(myPath,myFile);
if exist(filePath, 'file')
+
fileInfo = dir(filePath);
fileSize = fileInfo.bytes;
- fid = fopen(filePath, 'r');
- elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4))
+ elements = int64(fileSize./(lineLength))
- for i=1:elements
- % timestamp
- sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-
- % gyro (3 channels)
- sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
-
- % accelerometer (3 channels)
- sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
-
- % mag (3 channels)
- sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
-
- % baro pressure
- sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
-
- % baro alt
- sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
-
- % baro temp
- sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
-
- % actuator control (4 channels)
- sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
-
- % actuator outputs (8 channels)
- sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
-
- % vbat
- sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
-
- % adc voltage (3 channels)
- sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
-
- % local position (3 channels)
- sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
-
- % gps_raw_position (3 channels)
- sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
-
- % attitude (3 channels)
- sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
-
- % RotMatrix (9 channels)
- sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
-
- % vicon position (6 channels)
- sensors(i,48:53) = fread(fid, 6, 'float', 0, 'ieee-le');
-
- % actuator control effective (4 channels)
- sensors(i,54:57) = fread(fid, 4, 'float', 0, 'ieee-le');
-
- % optical flow (6 values)
- sensors(i,58:63) = fread(fid, 6, 'float', 0, 'ieee-le');
+ fid = fopen(filePath, 'r');
+ offset = 0;
+ for i=1:columns
+ % using fread with a skip speeds up the import drastically, do not
+ % import the values one after the other
+ sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
+ fid, ...
+ [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
+ lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
+ logFormat{i}.machineformat) ...
+ );
+ offset = offset + logFormat{i}.bytes*logFormat{i}.array;
+ fseek(fid, offset,'bof');
end
- time_us = sensors(elements,1) - sensors(1,1);
+
+ % shot the flight time
+ time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
time_s = time_us*1e-6
time_m = time_s/60
+
+ % close the logfile
+ fclose(fid);
+
disp(['end log2matlab conversion' char(10)]);
else
disp(['file: ' filePath ' does not exist' char(10)]);
end
-%% old version of reading in different files from sdlog.c
-% if exist('sysvector.bin', 'file')
-% % Read actuators file
-% myFile = java.io.File('sysvector.bin')
-% fileSize = length(myFile)
-%
-% fid = fopen('sysvector.bin', 'r');
-% elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
-%
-% for i=1:elements
-% % timestamp
-% sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-% % actuators 1-16
-% % quadrotor: motor 1-4 on the first four positions
-% sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
-% sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
-% end
-%
-% sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
-% sysvector_minutes = sysvector_interval_seconds / 60
-%
-% % Normalize time
-% sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
-%
-% % Create some basic plots
-%
-% % Remove zero rows from GPS
-% gps = sysvector(:,33:35);
-% gps(~any(gps,2), :) = [];
-%
-% all_data = figure('Name', 'GPS RAW');
-% gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
-%
-%
-% all_data = figure('Name', 'Complete Log Data (exc. GPS)');
-% plot(sysvector(:,1), sysvector(:,2:32));
-%
-% actuator_inputs = figure('Name', 'Attitude controller outputs');
-% plot(sysvector(:,1), sysvector(:,14:17));
-% legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
-%
-% actuator_outputs = figure('Name', 'Actuator outputs');
-% plot(sysvector(:,1), sysvector(:,18:25));
-% legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
-%
-% end
-%
-% if exist('actuator_outputs0.bin', 'file')
-% % Read actuators file
-% myFile = java.io.File('actuator_outputs0.bin')
-% fileSize = length(myFile)
-%
-% fid = fopen('actuator_outputs0.bin', 'r');
-% elements = int64(fileSize./(16*4+8))
-%
-% for i=1:elements
-% % timestamp
-% actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-% % actuators 1-16
-% % quadrotor: motor 1-4 on the first four positions
-% actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
-% end
-% end
-%
-% if exist('actuator_controls0.bin', 'file')
-% % Read actuators file
-% myFile = java.io.File('actuator_controls0.bin')
-% fileSize = length(myFile)
-%
-% fid = fopen('actuator_controls0.bin', 'r');
-% elements = int64(fileSize./(8*4+8))
-%
-% for i=1:elements
-% % timestamp
-% actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
-% % actuators 1-16
-% % quadrotor: motor 1-4 on the first four positions
-% actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
-% end
-% end
-%
-%
-% if exist('sensor_combined.bin', 'file')
-% % Read sensor combined file
-% % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
-% % Struct: sensor_combined_s
-% fileInfo = dir('sensor_combined.bin');
-% fileSize = fileInfo.bytes;
-%
-% fid = fopen('sensor_combined.bin', 'r');
-%
-% for i=1:elements
-% % timestamp
-% sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-% % gyro raw
-% sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
-% % gyro counter
-% sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
-% % gyro in rad/s
-% sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
-%
-% % accelerometer raw
-% sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
-% % padding bytes
-% fread(fid, 1, 'int16', 0, 'ieee-le');
-% % accelerometer counter
-% sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
-% % accel in m/s2
-% sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
-% % accel mode
-% sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
-% % accel range
-% sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
-%
-% % mag raw
-% sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
-% % padding bytes
-% fread(fid, 1, 'int16', 0, 'ieee-le');
-% % mag in Gauss
-% sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
-% % mag mode
-% sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
-% % mag range
-% sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
-% % mag cuttoff freq
-% sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
-% % mag counter
-% sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
-%
-% % baro pressure millibar
-% % baro alt meter
-% % baro temp celcius
-% % battery voltage
-% % adc voltage (3 channels)
-% sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
-% % baro counter and battery counter
-% sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
-% % battery voltage valid flag
-% sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
-%
-% end
-% end
-
-
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 5d928264d..3734d7755 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -1080,10 +1080,10 @@ int HMC5883::check_offset()
int HMC5883::check_calibration()
{
- bool offset_valid = !(check_offset() == OK);
- bool scale_valid = !(check_scale() == OK);
+ bool offset_valid = (check_offset() == OK);
+ bool scale_valid = (check_scale() == OK);
- if (_calibrated != (offset_valid && scale_valid == OK)) {
+ if (_calibrated != (offset_valid && scale_valid)) {
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index d5bd103c1..60d20905a 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -60,10 +60,19 @@ static struct hrt_call failsafe_call;
*/
static unsigned counter;
+/*
+ * Define the various LED flash sequences for each system state.
+ */
+#define LED_PATTERN_SAFE 0xffff // always on
+#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking
+#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking
+#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink
+
+static unsigned blink_counter = 0;
+
#define ARM_COUNTER_THRESHOLD 10
#define DISARM_COUNTER_THRESHOLD 2
-static bool safety_led_state;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
@@ -120,15 +129,25 @@ safety_check_button(void *arg)
counter = 0;
}
- /* when armed, toggle the LED; when safe, leave it on */
+ /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
+ uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) {
- safety_led_state = !safety_led_state;
- } else {
- safety_led_state = true;
+ if (system_state.arm_ok) {
+ pattern = LED_PATTERN_IO_FMU_ARMED;
+ } else {
+ pattern = LED_PATTERN_IO_ARMED;
+ }
+ } else if (system_state.arm_ok) {
+ pattern = LED_PATTERN_FMU_ARMED;
}
- LED_SAFETY(safety_led_state);
-}
+ /* Turn the LED on if we have a 1 at the current bit position */
+ LED_SAFETY(pattern & (1 << blink_counter++));
+
+ if (blink_counter > 15) {
+ blink_counter = 0;
+ }
+}
static void
heartbeat_blink(void *arg)
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 09:05:44 +0000