From 82cbac70ee70e67aa7e8e09af8fa47c462f6e62a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Dec 2012 17:07:42 +0100 Subject: [PATCH 1/4] Fixed calibration check --- apps/drivers/hmc5883/hmc5883.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 5d928264dc..3734d77552 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); From 197e573885ddc7c33e2b9b534a256b29fa7e3e1c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 7 Dec 2012 21:34:41 +0100 Subject: [PATCH 2/4] Add an additional safety switch LED blink sequence when FMU and IO are armed If both the FMU and the IO board are armed then the secure switch will blink two times quickly then a pause followed by two quick blinks and so on. --- apps/px4io/safety.c | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index d5bd103c10..761a1e1e75 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -60,6 +60,11 @@ static struct hrt_call failsafe_call; */ static unsigned counter; +/* + * Used to coordinate a special blink pattern wheb both the FMU and IO are armed. + */ +static unsigned blink_count = 0; + #define ARM_COUNTER_THRESHOLD 10 #define DISARM_COUNTER_THRESHOLD 2 @@ -120,9 +125,25 @@ safety_check_button(void *arg) counter = 0; } - /* when armed, toggle the LED; when safe, leave it on */ + /* + * When the IO is armed, toggle the LED; when IO and FMU armed use aircraft like + * pattern (long pause then 2 fast blinks); when safe, leave it on. + */ if (system_state.armed) { - safety_led_state = !safety_led_state; + if (system_state.arm_ok) { + /* FMU and IO are armed */ + if (blink_count > 9) { + safety_led_state = !safety_led_state; + } else { + safety_led_state = false; + } + if (blink_count++ == 12) { + blink_count = 0; + } + } else { + /* Only the IO is armed so use a constant blink rate */ + safety_led_state = !safety_led_state; + } } else { safety_led_state = true; } From 03b51c69e0b01e451c24e7c1158eca254ec69765 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 8 Dec 2012 13:39:28 +0100 Subject: [PATCH 3/4] Added more LED state logic and improve code. The LED will now also indicate when the FMU is ARMED. Switched to using a 16-bit value where each bit indicates what state the LED should be in. --- apps/px4io/safety.c | 42 ++++++++++++++++++++---------------------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 761a1e1e75..60d20905ab 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -61,14 +61,18 @@ static struct hrt_call failsafe_call; static unsigned counter; /* - * Used to coordinate a special blink pattern wheb both the FMU and IO are armed. + * Define the various LED flash sequences for each system state. */ -static unsigned blink_count = 0; +#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); @@ -125,31 +129,25 @@ safety_check_button(void *arg) counter = 0; } - /* - * When the IO is armed, toggle the LED; when IO and FMU armed use aircraft like - * pattern (long pause then 2 fast blinks); 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) { if (system_state.arm_ok) { - /* FMU and IO are armed */ - if (blink_count > 9) { - safety_led_state = !safety_led_state; - } else { - safety_led_state = false; - } - if (blink_count++ == 12) { - blink_count = 0; - } + pattern = LED_PATTERN_IO_FMU_ARMED; } else { - /* Only the IO is armed so use a constant blink rate */ - safety_led_state = !safety_led_state; + pattern = LED_PATTERN_IO_ARMED; } - } else { - safety_led_state = true; + } 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) From 4bc3f800437f8fdf7129fee9a85cc8faf11b7870 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Dec 2012 00:01:59 -0800 Subject: [PATCH 4/4] Greatly sped up Matlab import script, a 17min flight now takes 2secs to import instead of more than the actual flight time --- ROMFS/logging/logconv.m | 266 +++++++++------------------------------- 1 file changed, 61 insertions(+), 205 deletions(-) diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 48399f1ebd..bf70d6d0fa 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 - -