forked from Archive/PX4-Autopilot
Merged
This commit is contained in:
commit
64925c33cd
|
@ -11,6 +11,7 @@ Make.dep
|
||||||
*.o
|
*.o
|
||||||
*.a
|
*.a
|
||||||
*~
|
*~
|
||||||
|
*.dSYM
|
||||||
Images/*.bin
|
Images/*.bin
|
||||||
Images/*.px4
|
Images/*.px4
|
||||||
nuttx/Make.defs
|
nuttx/Make.defs
|
||||||
|
@ -40,3 +41,4 @@ nsh_romfsimg.h
|
||||||
cscope.out
|
cscope.out
|
||||||
.configX-e
|
.configX-e
|
||||||
nuttx-export.zip
|
nuttx-export.zip
|
||||||
|
dot.gdbinit
|
||||||
|
|
|
@ -1,6 +1,21 @@
|
||||||
|
% 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
|
clear all
|
||||||
close all
|
close all
|
||||||
|
|
||||||
|
% Set the path to your sysvector.bin file here
|
||||||
|
filePath = 'sysvector.bin';
|
||||||
|
|
||||||
|
% Work around a Matlab bug (not related to PX4)
|
||||||
|
% where timestamps from 1.1.1970 do not allow to
|
||||||
|
% read the file's size
|
||||||
|
if ismac
|
||||||
|
system('touch -t 201212121212.12 sysvector.bin');
|
||||||
|
end
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%
|
%%%%%%%%%%%%%%%%%%%%%%%
|
||||||
% SYSTEM VECTOR
|
% SYSTEM VECTOR
|
||||||
%
|
%
|
||||||
|
@ -16,229 +31,84 @@ close all
|
||||||
% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
|
% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
|
||||||
% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
|
% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
|
||||||
% float vbat; //battery voltage in [volt]
|
% float vbat; //battery voltage in [volt]
|
||||||
|
% float bat_current - current drawn from battery at this time instant
|
||||||
|
% float bat_discharged - discharged energy in mAh
|
||||||
% float adc[3]; //remaining auxiliary ADC ports [volt]
|
% float adc[3]; //remaining auxiliary ADC ports [volt]
|
||||||
% float local_position[3]; //tangent plane mapping into x,y,z [m]
|
% float local_position[3]; //tangent plane mapping into x,y,z [m]
|
||||||
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
|
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
|
||||||
% float attitude[3]; //pitch, roll, yaw [rad]
|
% float attitude[3]; //pitch, roll, yaw [rad]
|
||||||
% float rotMatrix[9]; //unitvectors
|
% float rotMatrix[9]; //unitvectors
|
||||||
|
% float actuator_control[4]; //unitvector
|
||||||
|
% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
|
||||||
|
% float diff_pressure; - pressure difference in millibar
|
||||||
|
% float ind_airspeed;
|
||||||
|
% float true_airspeed;
|
||||||
|
|
||||||
|
% 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', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
|
||||||
|
logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, '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')
|
if exist(filePath, 'file')
|
||||||
|
|
||||||
fileInfo = dir(filePath);
|
fileInfo = dir(filePath);
|
||||||
fileSize = fileInfo.bytes;
|
fileSize = fileInfo.bytes;
|
||||||
|
|
||||||
fid = fopen(filePath, 'r');
|
elements = int64(fileSize./(lineLength))
|
||||||
elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4))
|
|
||||||
|
|
||||||
for i=1:elements
|
fid = fopen(filePath, 'r');
|
||||||
% timestamp
|
offset = 0;
|
||||||
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
|
for i=1:columns
|
||||||
|
% using fread with a skip speeds up the import drastically, do not
|
||||||
% gyro (3 channels)
|
% import the values one after the other
|
||||||
sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
|
sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
|
||||||
|
fid, ...
|
||||||
% accelerometer (3 channels)
|
[logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
|
||||||
sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
|
lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
|
||||||
|
logFormat{i}.machineformat) ...
|
||||||
% mag (3 channels)
|
);
|
||||||
sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
|
offset = offset + logFormat{i}.bytes*logFormat{i}.array;
|
||||||
|
fseek(fid, offset,'bof');
|
||||||
% 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');
|
|
||||||
end
|
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_s = time_us*1e-6
|
||||||
time_m = time_s/60
|
time_m = time_s/60
|
||||||
|
|
||||||
|
% close the logfile
|
||||||
|
fclose(fid);
|
||||||
|
|
||||||
disp(['end log2matlab conversion' char(10)]);
|
disp(['end log2matlab conversion' char(10)]);
|
||||||
else
|
else
|
||||||
disp(['file: ' filePath ' does not exist' char(10)]);
|
disp(['file: ' filePath ' does not exist' char(10)]);
|
||||||
end
|
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
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1,12 +1,9 @@
|
||||||
#!nsh
|
#!nsh
|
||||||
#
|
|
||||||
# Flight startup script for PX4FMU with PX4IO carrier board.
|
|
||||||
#
|
|
||||||
|
|
||||||
echo "[init] doing PX4IO startup..."
|
set USB no
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start the ORB
|
# Start the object request broker
|
||||||
#
|
#
|
||||||
uorb start
|
uorb start
|
||||||
|
|
||||||
|
@ -20,15 +17,27 @@ then
|
||||||
param load
|
param load
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
#
|
||||||
|
# Enable / connect to PX4IO
|
||||||
|
#
|
||||||
|
px4io start
|
||||||
|
|
||||||
|
#
|
||||||
|
# Load an appropriate mixer. FMU_pass.mix is a passthru mixer
|
||||||
|
# which is good for testing. See ROMFS/mixers for a full list of mixers.
|
||||||
|
#
|
||||||
|
mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start the sensors.
|
# Start the sensors.
|
||||||
#
|
#
|
||||||
sh /etc/init.d/rc.sensors
|
sh /etc/init.d/rc.sensors
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start MAVLink
|
# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable)
|
||||||
#
|
#
|
||||||
mavlink start -d /dev/ttyS0 -b 57600
|
mavlink start -d /dev/ttyS0 -b 57600
|
||||||
|
usleep 5000
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start the commander.
|
# Start the commander.
|
||||||
|
@ -41,35 +50,24 @@ commander start
|
||||||
attitude_estimator_ekf start
|
attitude_estimator_ekf start
|
||||||
|
|
||||||
#
|
#
|
||||||
# Configure PX4FMU for operation with PX4IO
|
# Start the attitude and position controller
|
||||||
#
|
#
|
||||||
# XXX arguments?
|
fixedwing_att_control start
|
||||||
#
|
fixedwing_pos_control start
|
||||||
px4fmu start
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start the fixed-wing controller
|
# Start GPS capture. Comment this out if you do not have a GPS.
|
||||||
#
|
|
||||||
fixedwing_control start
|
|
||||||
|
|
||||||
#
|
|
||||||
# Fire up the PX4IO interface.
|
|
||||||
#
|
|
||||||
px4io start
|
|
||||||
|
|
||||||
#
|
|
||||||
# Start looking for a GPS.
|
|
||||||
#
|
#
|
||||||
gps start
|
gps start
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start logging to microSD if we can
|
# Start logging to microSD if we can
|
||||||
#
|
#
|
||||||
sh /etc/init.d/rc.logging
|
sh /etc/init.d/rc.logging
|
||||||
|
|
||||||
#
|
#
|
||||||
# startup is done; we don't want the shell because we
|
# startup is done; we don't want the shell because we
|
||||||
# use the same UART for telemetry (dumb).
|
# use the same UART for telemetry
|
||||||
#
|
#
|
||||||
echo "[init] startup done, exiting."
|
echo "[init] startup done"
|
||||||
exit
|
exit
|
||||||
|
|
|
@ -3,8 +3,12 @@
|
||||||
# Flight startup script for PX4FMU on PX4IOAR carrier board.
|
# Flight startup script for PX4FMU on PX4IOAR carrier board.
|
||||||
#
|
#
|
||||||
|
|
||||||
|
# Disable the USB interface
|
||||||
set USB no
|
set USB no
|
||||||
|
|
||||||
|
# Disable autostarting other apps
|
||||||
|
set MODE ardrone
|
||||||
|
|
||||||
echo "[init] doing PX4IOAR startup..."
|
echo "[init] doing PX4IOAR startup..."
|
||||||
|
|
||||||
#
|
#
|
||||||
|
@ -13,26 +17,26 @@ echo "[init] doing PX4IOAR startup..."
|
||||||
uorb start
|
uorb start
|
||||||
|
|
||||||
#
|
#
|
||||||
# Init the EEPROM
|
# Load microSD params
|
||||||
#
|
#
|
||||||
echo "[init] eeprom"
|
echo "[init] loading microSD params"
|
||||||
eeprom start
|
param select /fs/microsd/parameters
|
||||||
if [ -f /eeprom/parameters ]
|
if [ -f /fs/microsd/parameters ]
|
||||||
then
|
then
|
||||||
param load
|
param load /fs/microsd/parameters
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#
|
|
||||||
# Start the sensors.
|
|
||||||
#
|
|
||||||
sh /etc/init.d/rc.sensors
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start MAVLink
|
# Start MAVLink
|
||||||
#
|
#
|
||||||
mavlink start -d /dev/ttyS0 -b 57600
|
mavlink start -d /dev/ttyS0 -b 57600
|
||||||
usleep 5000
|
usleep 5000
|
||||||
|
|
||||||
|
#
|
||||||
|
# Start the sensors and test them.
|
||||||
|
#
|
||||||
|
sh /etc/init.d/rc.sensors
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start the commander.
|
# Start the commander.
|
||||||
#
|
#
|
||||||
|
@ -56,13 +60,13 @@ multirotor_att_control start
|
||||||
#
|
#
|
||||||
# Fire up the AR.Drone interface.
|
# Fire up the AR.Drone interface.
|
||||||
#
|
#
|
||||||
ardrone_interface start
|
ardrone_interface start -d /dev/ttyS1
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start logging to microSD if we can
|
# Start logging
|
||||||
#
|
#
|
||||||
#sh /etc/init.d/rc.logging
|
#sdlog start
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start GPS capture
|
# Start GPS capture
|
||||||
#
|
#
|
||||||
|
|
|
@ -8,6 +8,7 @@
|
||||||
#
|
#
|
||||||
|
|
||||||
ms5611 start
|
ms5611 start
|
||||||
|
adc start
|
||||||
|
|
||||||
if mpu6000 start
|
if mpu6000 start
|
||||||
then
|
then
|
||||||
|
|
|
@ -46,8 +46,12 @@ if [ -f /fs/microsd/etc/rc ]
|
||||||
then
|
then
|
||||||
echo "[init] reading /fs/microsd/etc/rc"
|
echo "[init] reading /fs/microsd/etc/rc"
|
||||||
sh /fs/microsd/etc/rc
|
sh /fs/microsd/etc/rc
|
||||||
else
|
fi
|
||||||
echo "[init] script /fs/microsd/etc/rc not present"
|
# Also consider rc.txt files
|
||||||
|
if [ -f /fs/microsd/etc/rc.txt ]
|
||||||
|
then
|
||||||
|
echo "[init] reading /fs/microsd/etc/rc.txt"
|
||||||
|
sh /fs/microsd/etc/rc.txt
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|
|
@ -0,0 +1,19 @@
|
||||||
|
#!/bin/sh
|
||||||
|
astyle \
|
||||||
|
--style=linux \
|
||||||
|
--indent=force-tab=8 \
|
||||||
|
--indent-cases \
|
||||||
|
--indent-preprocessor \
|
||||||
|
--break-blocks=all \
|
||||||
|
--pad-oper \
|
||||||
|
--pad-header \
|
||||||
|
--unpad-paren \
|
||||||
|
--keep-one-line-blocks \
|
||||||
|
--keep-one-line-statements \
|
||||||
|
--align-pointer=name \
|
||||||
|
--suffix=none \
|
||||||
|
--lineend=linux \
|
||||||
|
$*
|
||||||
|
#--ignore-exclude-errors-x \
|
||||||
|
#--exclude=EASTL \
|
||||||
|
#--align-reference=name \
|
|
@ -112,6 +112,9 @@ endef
|
||||||
|
|
||||||
$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
|
$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
|
||||||
|
|
||||||
|
# EXTERNAL_APPS is used to add out of tree apps to the build
|
||||||
|
INSTALLED_APPS += $(EXTERNAL_APPS)
|
||||||
|
|
||||||
# The external/ directory may also be added to the INSTALLED_APPS. But there
|
# The external/ directory may also be added to the INSTALLED_APPS. But there
|
||||||
# is no external/ directory in the repository. Rather, this directory may be
|
# is no external/ directory in the repository. Rather, this directory may be
|
||||||
# provided by the user (possibly as a symbolic link) to add libraries and
|
# provided by the user (possibly as a symbolic link) to add libraries and
|
||||||
|
|
|
@ -35,7 +35,7 @@
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* @file attitude_estimator_ekf_main.c
|
* @file attitude_estimator_ekf_main.c
|
||||||
*
|
*
|
||||||
* Extended Kalman Filter for Attitude Estimation.
|
* Extended Kalman Filter for Attitude Estimation.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
@ -79,18 +79,18 @@ static float dt = 0.005f;
|
||||||
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
|
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
|
||||||
static float x_aposteriori_k[12]; /**< states */
|
static float x_aposteriori_k[12]; /**< states */
|
||||||
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
||||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
||||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
||||||
}; /**< init: diagonal matrix with big values */
|
}; /**< init: diagonal matrix with big values */
|
||||||
|
|
||||||
static float x_aposteriori[12];
|
static float x_aposteriori[12];
|
||||||
static float P_aposteriori[144];
|
static float P_aposteriori[144];
|
||||||
|
@ -99,9 +99,9 @@ static float P_aposteriori[144];
|
||||||
static float euler[3] = {0.0f, 0.0f, 0.0f};
|
static float euler[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
|
||||||
static float Rot_matrix[9] = {1.f, 0, 0,
|
static float Rot_matrix[9] = {1.f, 0, 0,
|
||||||
0, 1.f, 0,
|
0, 1.f, 0,
|
||||||
0, 0, 1.f
|
0, 0, 1.f
|
||||||
}; /**< init: identity matrix */
|
}; /**< init: identity matrix */
|
||||||
|
|
||||||
|
|
||||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||||
|
@ -123,6 +123,7 @@ usage(const char *reason)
|
||||||
{
|
{
|
||||||
if (reason)
|
if (reason)
|
||||||
fprintf(stderr, "%s\n", reason);
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
|
||||||
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
|
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
@ -131,7 +132,7 @@ usage(const char *reason)
|
||||||
* The attitude_estimator_ekf app only briefly exists to start
|
* The attitude_estimator_ekf app only briefly exists to start
|
||||||
* the background job. The stack size assigned in the
|
* the background job. The stack size assigned in the
|
||||||
* Makefile does only apply to this management task.
|
* Makefile does only apply to this management task.
|
||||||
*
|
*
|
||||||
* The actual stack size should be set in the call
|
* The actual stack size should be set in the call
|
||||||
* to task_create().
|
* to task_create().
|
||||||
*/
|
*/
|
||||||
|
@ -150,11 +151,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
|
||||||
|
|
||||||
thread_should_exit = false;
|
thread_should_exit = false;
|
||||||
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
|
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_MAX - 5,
|
SCHED_PRIORITY_MAX - 5,
|
||||||
12000,
|
12000,
|
||||||
attitude_estimator_ekf_thread_main,
|
attitude_estimator_ekf_thread_main,
|
||||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -166,9 +167,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (thread_running) {
|
if (thread_running) {
|
||||||
printf("\tattitude_estimator_ekf app is running\n");
|
printf("\tattitude_estimator_ekf app is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("\tattitude_estimator_ekf app not started\n");
|
printf("\tattitude_estimator_ekf app not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -235,7 +238,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
/* advertise debug value */
|
/* advertise debug value */
|
||||||
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
||||||
// orb_advert_t pub_dbg = -1;
|
// orb_advert_t pub_dbg = -1;
|
||||||
|
|
||||||
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
|
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
|
||||||
// XXX write this out to perf regs
|
// XXX write this out to perf regs
|
||||||
|
|
||||||
|
@ -263,8 +266,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
while (!thread_should_exit) {
|
while (!thread_should_exit) {
|
||||||
|
|
||||||
struct pollfd fds[2] = {
|
struct pollfd fds[2] = {
|
||||||
{ .fd = sub_raw, .events = POLLIN },
|
{ .fd = sub_raw, .events = POLLIN },
|
||||||
{ .fd = sub_params, .events = POLLIN }
|
{ .fd = sub_params, .events = POLLIN }
|
||||||
};
|
};
|
||||||
int ret = poll(fds, 2, 1000);
|
int ret = poll(fds, 2, 1000);
|
||||||
|
|
||||||
|
@ -273,10 +276,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
} else if (ret == 0) {
|
} else if (ret == 0) {
|
||||||
/* check if we're in HIL - not getting sensor data is fine then */
|
/* check if we're in HIL - not getting sensor data is fine then */
|
||||||
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
|
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
|
||||||
|
|
||||||
if (!state.flag_hil_enabled) {
|
if (!state.flag_hil_enabled) {
|
||||||
fprintf(stderr,
|
fprintf(stderr,
|
||||||
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
|
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
/* only update parameters if they changed */
|
/* only update parameters if they changed */
|
||||||
|
@ -308,6 +313,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
gyro_offsets[1] /= offset_count;
|
gyro_offsets[1] /= offset_count;
|
||||||
gyro_offsets[2] /= offset_count;
|
gyro_offsets[2] /= offset_count;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
perf_begin(ekf_loop_perf);
|
perf_begin(ekf_loop_perf);
|
||||||
|
@ -336,6 +342,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||||
sensor_last_timestamp[1] = raw.timestamp;
|
sensor_last_timestamp[1] = raw.timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
z_k[3] = raw.accelerometer_m_s2[0];
|
z_k[3] = raw.accelerometer_m_s2[0];
|
||||||
z_k[4] = raw.accelerometer_m_s2[1];
|
z_k[4] = raw.accelerometer_m_s2[1];
|
||||||
z_k[5] = raw.accelerometer_m_s2[2];
|
z_k[5] = raw.accelerometer_m_s2[2];
|
||||||
|
@ -347,6 +354,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||||
sensor_last_timestamp[2] = raw.timestamp;
|
sensor_last_timestamp[2] = raw.timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
z_k[6] = raw.magnetometer_ga[0];
|
z_k[6] = raw.magnetometer_ga[0];
|
||||||
z_k[7] = raw.magnetometer_ga[1];
|
z_k[7] = raw.magnetometer_ga[1];
|
||||||
z_k[8] = raw.magnetometer_ga[2];
|
z_k[8] = raw.magnetometer_ga[2];
|
||||||
|
@ -368,8 +376,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
static bool const_initialized = false;
|
static bool const_initialized = false;
|
||||||
|
|
||||||
/* initialize with good values once we have a reasonable dt estimate */
|
/* initialize with good values once we have a reasonable dt estimate */
|
||||||
if (!const_initialized && dt < 0.05f && dt > 0.005f)
|
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
|
||||||
{
|
|
||||||
dt = 0.005f;
|
dt = 0.005f;
|
||||||
parameters_update(&ekf_param_handles, &ekf_params);
|
parameters_update(&ekf_param_handles, &ekf_params);
|
||||||
|
|
||||||
|
@ -395,13 +402,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t timing_start = hrt_absolute_time();
|
uint64_t timing_start = hrt_absolute_time();
|
||||||
|
|
||||||
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
|
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
|
||||||
euler, Rot_matrix, x_aposteriori, P_aposteriori);
|
euler, Rot_matrix, x_aposteriori, P_aposteriori);
|
||||||
|
|
||||||
/* swap values for next iteration, check for fatal inputs */
|
/* swap values for next iteration, check for fatal inputs */
|
||||||
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
|
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
|
||||||
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
|
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
|
||||||
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
|
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* due to inputs or numerical failure the output is invalid, skip it */
|
/* due to inputs or numerical failure the output is invalid, skip it */
|
||||||
continue;
|
continue;
|
||||||
|
@ -413,9 +422,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* send out */
|
/* send out */
|
||||||
att.timestamp = raw.timestamp;
|
att.timestamp = raw.timestamp;
|
||||||
att.roll = euler[0];
|
|
||||||
att.pitch = euler[1];
|
// XXX Apply the same transformation to the rotation matrix
|
||||||
att.yaw = euler[2];
|
att.roll = euler[0] - ekf_params.roll_off;
|
||||||
|
att.pitch = euler[1] - ekf_params.pitch_off;
|
||||||
|
att.yaw = euler[2] - ekf_params.yaw_off;
|
||||||
|
|
||||||
att.rollspeed = x_aposteriori[0];
|
att.rollspeed = x_aposteriori[0];
|
||||||
att.pitchspeed = x_aposteriori[1];
|
att.pitchspeed = x_aposteriori[1];
|
||||||
|
@ -431,6 +442,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||||
// Broadcast
|
// Broadcast
|
||||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
warnx("NaN in roll/pitch/yaw estimate!");
|
warnx("NaN in roll/pitch/yaw estimate!");
|
||||||
}
|
}
|
||||||
|
|
|
@ -35,7 +35,7 @@
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* @file attitude_estimator_ekf_params.c
|
* @file attitude_estimator_ekf_params.c
|
||||||
*
|
*
|
||||||
* Parameters for EKF filter
|
* Parameters for EKF filter
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
@ -65,13 +65,17 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
|
||||||
/* accelerometer measurement noise */
|
/* accelerometer measurement noise */
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f);
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f);
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
|
||||||
/* magnetometer measurement noise */
|
/* magnetometer measurement noise */
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
|
||||||
PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
|
PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
|
||||||
|
/* offsets in roll, pitch and yaw of sensor plane and body */
|
||||||
|
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
|
||||||
|
|
||||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
||||||
{
|
{
|
||||||
|
@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
||||||
h->r7 = param_find("EKF_ATT_R7");
|
h->r7 = param_find("EKF_ATT_R7");
|
||||||
h->r8 = param_find("EKF_ATT_R8");
|
h->r8 = param_find("EKF_ATT_R8");
|
||||||
|
|
||||||
|
h->roll_off = param_find("ATT_ROLL_OFFS");
|
||||||
|
h->pitch_off = param_find("ATT_PITCH_OFFS");
|
||||||
|
h->yaw_off = param_find("ATT_YAW_OFFS");
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
|
||||||
param_get(h->r7, &(p->r[7]));
|
param_get(h->r7, &(p->r[7]));
|
||||||
param_get(h->r8, &(p->r[8]));
|
param_get(h->r8, &(p->r[8]));
|
||||||
|
|
||||||
|
param_get(h->roll_off, &(p->roll_off));
|
||||||
|
param_get(h->pitch_off, &(p->pitch_off));
|
||||||
|
param_get(h->yaw_off, &(p->yaw_off));
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
|
@ -35,7 +35,7 @@
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* @file attitude_estimator_ekf_params.h
|
* @file attitude_estimator_ekf_params.h
|
||||||
*
|
*
|
||||||
* Parameters for EKF filter
|
* Parameters for EKF filter
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
@ -44,11 +44,15 @@
|
||||||
struct attitude_estimator_ekf_params {
|
struct attitude_estimator_ekf_params {
|
||||||
float r[9];
|
float r[9];
|
||||||
float q[12];
|
float q[12];
|
||||||
|
float roll_off;
|
||||||
|
float pitch_off;
|
||||||
|
float yaw_off;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct attitude_estimator_ekf_param_handles {
|
struct attitude_estimator_ekf_param_handles {
|
||||||
param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
|
param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
|
||||||
param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
|
param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
|
||||||
|
param_t roll_off, pitch_off, yaw_off;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -45,172 +45,175 @@
|
||||||
|
|
||||||
|
|
||||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) {
|
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
|
||||||
|
{
|
||||||
|
|
||||||
float x_sumplain = 0.0f;
|
float x_sumplain = 0.0f;
|
||||||
float x_sumsq = 0.0f;
|
float x_sumsq = 0.0f;
|
||||||
float x_sumcube = 0.0f;
|
float x_sumcube = 0.0f;
|
||||||
|
|
||||||
float y_sumplain = 0.0f;
|
float y_sumplain = 0.0f;
|
||||||
float y_sumsq = 0.0f;
|
float y_sumsq = 0.0f;
|
||||||
float y_sumcube = 0.0f;
|
float y_sumcube = 0.0f;
|
||||||
|
|
||||||
float z_sumplain = 0.0f;
|
float z_sumplain = 0.0f;
|
||||||
float z_sumsq = 0.0f;
|
float z_sumsq = 0.0f;
|
||||||
float z_sumcube = 0.0f;
|
float z_sumcube = 0.0f;
|
||||||
|
|
||||||
float xy_sum = 0.0f;
|
float xy_sum = 0.0f;
|
||||||
float xz_sum = 0.0f;
|
float xz_sum = 0.0f;
|
||||||
float yz_sum = 0.0f;
|
float yz_sum = 0.0f;
|
||||||
|
|
||||||
float x2y_sum = 0.0f;
|
float x2y_sum = 0.0f;
|
||||||
float x2z_sum = 0.0f;
|
float x2z_sum = 0.0f;
|
||||||
float y2x_sum = 0.0f;
|
float y2x_sum = 0.0f;
|
||||||
float y2z_sum = 0.0f;
|
float y2z_sum = 0.0f;
|
||||||
float z2x_sum = 0.0f;
|
float z2x_sum = 0.0f;
|
||||||
float z2y_sum = 0.0f;
|
float z2y_sum = 0.0f;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < size; i++) {
|
for (unsigned int i = 0; i < size; i++) {
|
||||||
|
|
||||||
float x2 = x[i] * x[i];
|
float x2 = x[i] * x[i];
|
||||||
float y2 = y[i] * y[i];
|
float y2 = y[i] * y[i];
|
||||||
float z2 = z[i] * z[i];
|
float z2 = z[i] * z[i];
|
||||||
|
|
||||||
x_sumplain += x[i];
|
x_sumplain += x[i];
|
||||||
x_sumsq += x2;
|
x_sumsq += x2;
|
||||||
x_sumcube += x2 * x[i];
|
x_sumcube += x2 * x[i];
|
||||||
|
|
||||||
y_sumplain += y[i];
|
y_sumplain += y[i];
|
||||||
y_sumsq += y2;
|
y_sumsq += y2;
|
||||||
y_sumcube += y2 * y[i];
|
y_sumcube += y2 * y[i];
|
||||||
|
|
||||||
z_sumplain += z[i];
|
z_sumplain += z[i];
|
||||||
z_sumsq += z2;
|
z_sumsq += z2;
|
||||||
z_sumcube += z2 * z[i];
|
z_sumcube += z2 * z[i];
|
||||||
|
|
||||||
xy_sum += x[i] * y[i];
|
xy_sum += x[i] * y[i];
|
||||||
xz_sum += x[i] * z[i];
|
xz_sum += x[i] * z[i];
|
||||||
yz_sum += y[i] * z[i];
|
yz_sum += y[i] * z[i];
|
||||||
|
|
||||||
x2y_sum += x2 * y[i];
|
x2y_sum += x2 * y[i];
|
||||||
x2z_sum += x2 * z[i];
|
x2z_sum += x2 * z[i];
|
||||||
|
|
||||||
y2x_sum += y2 * x[i];
|
y2x_sum += y2 * x[i];
|
||||||
y2z_sum += y2 * z[i];
|
y2z_sum += y2 * z[i];
|
||||||
|
|
||||||
z2x_sum += z2 * x[i];
|
z2x_sum += z2 * x[i];
|
||||||
z2y_sum += z2 * y[i];
|
z2y_sum += z2 * y[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
|
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
|
||||||
//
|
//
|
||||||
// P is a structure that has been computed with the data earlier.
|
// P is a structure that has been computed with the data earlier.
|
||||||
// P.npoints is the number of elements; the length of X,Y,Z are identical.
|
// P.npoints is the number of elements; the length of X,Y,Z are identical.
|
||||||
// P's members are logically named.
|
// P's members are logically named.
|
||||||
//
|
//
|
||||||
// X[n] is the x component of point n
|
// X[n] is the x component of point n
|
||||||
// Y[n] is the y component of point n
|
// Y[n] is the y component of point n
|
||||||
// Z[n] is the z component of point n
|
// Z[n] is the z component of point n
|
||||||
//
|
//
|
||||||
// A is the x coordiante of the sphere
|
// A is the x coordiante of the sphere
|
||||||
// B is the y coordiante of the sphere
|
// B is the y coordiante of the sphere
|
||||||
// C is the z coordiante of the sphere
|
// C is the z coordiante of the sphere
|
||||||
// Rsq is the radius squared of the sphere.
|
// Rsq is the radius squared of the sphere.
|
||||||
//
|
//
|
||||||
//This method should converge; maybe 5-100 iterations or more.
|
//This method should converge; maybe 5-100 iterations or more.
|
||||||
//
|
//
|
||||||
float x_sum = x_sumplain / size; //sum( X[n] )
|
float x_sum = x_sumplain / size; //sum( X[n] )
|
||||||
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
|
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
|
||||||
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
|
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
|
||||||
float y_sum = y_sumplain / size; //sum( Y[n] )
|
float y_sum = y_sumplain / size; //sum( Y[n] )
|
||||||
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
|
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
|
||||||
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
|
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
|
||||||
float z_sum = z_sumplain / size; //sum( Z[n] )
|
float z_sum = z_sumplain / size; //sum( Z[n] )
|
||||||
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
|
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
|
||||||
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
|
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
|
||||||
|
|
||||||
float XY = xy_sum / size; //sum( X[n] * Y[n] )
|
float XY = xy_sum / size; //sum( X[n] * Y[n] )
|
||||||
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
|
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
|
||||||
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
|
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
|
||||||
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
|
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
|
||||||
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
|
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
|
||||||
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
|
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
|
||||||
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
|
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
|
||||||
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
|
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
|
||||||
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
|
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
|
||||||
|
|
||||||
//Reduction of multiplications
|
//Reduction of multiplications
|
||||||
float F0 = x_sum2 + y_sum2 + z_sum2;
|
float F0 = x_sum2 + y_sum2 + z_sum2;
|
||||||
float F1 = 0.5f * F0;
|
float F1 = 0.5f * F0;
|
||||||
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
|
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
|
||||||
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
|
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
|
||||||
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
|
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
|
||||||
|
|
||||||
//Set initial conditions:
|
//Set initial conditions:
|
||||||
float A = x_sum;
|
float A = x_sum;
|
||||||
float B = y_sum;
|
float B = y_sum;
|
||||||
float C = z_sum;
|
float C = z_sum;
|
||||||
|
|
||||||
//First iteration computation:
|
//First iteration computation:
|
||||||
float A2 = A*A;
|
float A2 = A * A;
|
||||||
float B2 = B*B;
|
float B2 = B * B;
|
||||||
float C2 = C*C;
|
float C2 = C * C;
|
||||||
float QS = A2 + B2 + C2;
|
float QS = A2 + B2 + C2;
|
||||||
float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
|
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||||
|
|
||||||
//Set initial conditions:
|
//Set initial conditions:
|
||||||
float Rsq = F0 + QB + QS;
|
float Rsq = F0 + QB + QS;
|
||||||
|
|
||||||
//First iteration computation:
|
//First iteration computation:
|
||||||
float Q0 = 0.5f * (QS - Rsq);
|
float Q0 = 0.5f * (QS - Rsq);
|
||||||
float Q1 = F1 + Q0;
|
float Q1 = F1 + Q0;
|
||||||
float Q2 = 8.0f * ( QS - Rsq + QB + F0 );
|
float Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||||
float aA,aB,aC,nA,nB,nC,dA,dB,dC;
|
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
|
||||||
|
|
||||||
//Iterate N times, ignore stop condition.
|
//Iterate N times, ignore stop condition.
|
||||||
int n = 0;
|
int n = 0;
|
||||||
while( n < max_iterations ){
|
|
||||||
n++;
|
|
||||||
|
|
||||||
//Compute denominator:
|
while (n < max_iterations) {
|
||||||
aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2);
|
n++;
|
||||||
aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2);
|
|
||||||
aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2);
|
|
||||||
aA = (aA == 0.0f) ? 1.0f : aA;
|
|
||||||
aB = (aB == 0.0f) ? 1.0f : aB;
|
|
||||||
aC = (aC == 0.0f) ? 1.0f : aC;
|
|
||||||
|
|
||||||
//Compute next iteration
|
//Compute denominator:
|
||||||
nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA);
|
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
|
||||||
nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB);
|
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
|
||||||
nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC);
|
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
|
||||||
|
aA = (aA == 0.0f) ? 1.0f : aA;
|
||||||
|
aB = (aB == 0.0f) ? 1.0f : aB;
|
||||||
|
aC = (aC == 0.0f) ? 1.0f : aC;
|
||||||
|
|
||||||
//Check for stop condition
|
//Compute next iteration
|
||||||
dA = (nA - A);
|
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
|
||||||
dB = (nB - B);
|
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
|
||||||
dC = (nC - C);
|
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
|
||||||
if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; }
|
|
||||||
|
|
||||||
//Compute next iteration's values
|
//Check for stop condition
|
||||||
A = nA;
|
dA = (nA - A);
|
||||||
B = nB;
|
dB = (nB - B);
|
||||||
C = nC;
|
dC = (nC - C);
|
||||||
A2 = A*A;
|
|
||||||
B2 = B*B;
|
|
||||||
C2 = C*C;
|
|
||||||
QS = A2 + B2 + C2;
|
|
||||||
QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
|
|
||||||
Rsq = F0 + QB + QS;
|
|
||||||
Q0 = 0.5f * (QS - Rsq);
|
|
||||||
Q1 = F1 + Q0;
|
|
||||||
Q2 = 8.0f * ( QS - Rsq + QB + F0 );
|
|
||||||
}
|
|
||||||
|
|
||||||
*sphere_x = A;
|
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
|
||||||
*sphere_y = B;
|
|
||||||
*sphere_z = C;
|
|
||||||
*sphere_radius = sqrtf(Rsq);
|
|
||||||
|
|
||||||
return 0;
|
//Compute next iteration's values
|
||||||
|
A = nA;
|
||||||
|
B = nB;
|
||||||
|
C = nC;
|
||||||
|
A2 = A * A;
|
||||||
|
B2 = B * B;
|
||||||
|
C2 = C * C;
|
||||||
|
QS = A2 + B2 + C2;
|
||||||
|
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||||
|
Rsq = F0 + QB + QS;
|
||||||
|
Q0 = 0.5f * (QS - Rsq);
|
||||||
|
Q1 = F1 + Q0;
|
||||||
|
Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||||
|
}
|
||||||
|
|
||||||
|
*sphere_x = A;
|
||||||
|
*sphere_y = B;
|
||||||
|
*sphere_z = C;
|
||||||
|
*sphere_radius = sqrtf(Rsq);
|
||||||
|
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -58,4 +58,4 @@
|
||||||
* @return 0 on success, 1 on failure
|
* @return 0 on success, 1 on failure
|
||||||
*/
|
*/
|
||||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
|
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
|
File diff suppressed because it is too large
Load Diff
|
@ -52,4 +52,7 @@
|
||||||
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
|
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
|
||||||
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
|
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
|
||||||
|
|
||||||
|
void tune_confirm(void);
|
||||||
|
void tune_error(void);
|
||||||
|
|
||||||
#endif /* COMMANDER_H_ */
|
#endif /* COMMANDER_H_ */
|
||||||
|
|
|
@ -50,7 +50,7 @@
|
||||||
|
|
||||||
#include "state_machine_helper.h"
|
#include "state_machine_helper.h"
|
||||||
|
|
||||||
static const char* system_state_txt[] = {
|
static const char *system_state_txt[] = {
|
||||||
"SYSTEM_STATE_PREFLIGHT",
|
"SYSTEM_STATE_PREFLIGHT",
|
||||||
"SYSTEM_STATE_STANDBY",
|
"SYSTEM_STATE_STANDBY",
|
||||||
"SYSTEM_STATE_GROUND_READY",
|
"SYSTEM_STATE_GROUND_READY",
|
||||||
|
@ -79,7 +79,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
case SYSTEM_STATE_MISSION_ABORT: {
|
case SYSTEM_STATE_MISSION_ABORT: {
|
||||||
/* Indoor or outdoor */
|
/* Indoor or outdoor */
|
||||||
// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
|
// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
|
||||||
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
|
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
|
||||||
|
|
||||||
// } else {
|
// } else {
|
||||||
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
|
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
|
||||||
|
@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* set system flags according to state */
|
/* set system flags according to state */
|
||||||
current_status->flag_system_armed = true;
|
current_status->flag_system_armed = true;
|
||||||
|
|
||||||
fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
|
warnx("EMERGENCY LANDING!\n");
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
|
mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_EMCY_CUTOFF:
|
case SYSTEM_STATE_EMCY_CUTOFF:
|
||||||
|
@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* set system flags according to state */
|
/* set system flags according to state */
|
||||||
current_status->flag_system_armed = false;
|
current_status->flag_system_armed = false;
|
||||||
|
|
||||||
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
|
warnx("EMERGENCY MOTOR CUTOFF!\n");
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
|
mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_GROUND_ERROR:
|
case SYSTEM_STATE_GROUND_ERROR:
|
||||||
|
@ -114,36 +114,40 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* prevent actuators from arming */
|
/* prevent actuators from arming */
|
||||||
current_status->flag_system_armed = false;
|
current_status->flag_system_armed = false;
|
||||||
|
|
||||||
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
|
warnx("GROUND ERROR, locking down propulsion system\n");
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
|
mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_PREFLIGHT:
|
case SYSTEM_STATE_PREFLIGHT:
|
||||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
||||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||||
/* set system flags according to state */
|
/* set system flags according to state */
|
||||||
current_status->flag_system_armed = false;
|
current_status->flag_system_armed = false;
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
|
mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
invalid_state = true;
|
invalid_state = true;
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
|
mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_REBOOT:
|
case SYSTEM_STATE_REBOOT:
|
||||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
||||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||||
invalid_state = false;
|
invalid_state = false;
|
||||||
/* set system flags according to state */
|
/* set system flags according to state */
|
||||||
current_status->flag_system_armed = false;
|
current_status->flag_system_armed = false;
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
|
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
|
||||||
usleep(500000);
|
usleep(500000);
|
||||||
up_systemreset();
|
up_systemreset();
|
||||||
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
|
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
invalid_state = true;
|
invalid_state = true;
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
|
mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_STANDBY:
|
case SYSTEM_STATE_STANDBY:
|
||||||
|
@ -152,7 +156,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* standby enforces disarmed */
|
/* standby enforces disarmed */
|
||||||
current_status->flag_system_armed = false;
|
current_status->flag_system_armed = false;
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
|
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_GROUND_READY:
|
case SYSTEM_STATE_GROUND_READY:
|
||||||
|
@ -162,7 +166,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* ground ready has motors / actuators armed */
|
/* ground ready has motors / actuators armed */
|
||||||
current_status->flag_system_armed = true;
|
current_status->flag_system_armed = true;
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
|
mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_AUTO:
|
case SYSTEM_STATE_AUTO:
|
||||||
|
@ -172,7 +176,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* auto is airborne and in auto mode, motors armed */
|
/* auto is airborne and in auto mode, motors armed */
|
||||||
current_status->flag_system_armed = true;
|
current_status->flag_system_armed = true;
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
|
mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_STABILIZED:
|
case SYSTEM_STATE_STABILIZED:
|
||||||
|
@ -180,7 +184,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* set system flags according to state */
|
/* set system flags according to state */
|
||||||
current_status->flag_system_armed = true;
|
current_status->flag_system_armed = true;
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
|
mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SYSTEM_STATE_MANUAL:
|
case SYSTEM_STATE_MANUAL:
|
||||||
|
@ -188,7 +192,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
/* set system flags according to state */
|
/* set system flags according to state */
|
||||||
current_status->flag_system_armed = true;
|
current_status->flag_system_armed = true;
|
||||||
|
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
|
mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -202,14 +206,17 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||||
publish_armed_status(current_status);
|
publish_armed_status(current_status);
|
||||||
ret = OK;
|
ret = OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (invalid_state) {
|
if (invalid_state) {
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
|
mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
|
||||||
ret = ERROR;
|
ret = ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
|
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
|
{
|
||||||
/* publish the new state */
|
/* publish the new state */
|
||||||
current_status->counter++;
|
current_status->counter++;
|
||||||
current_status->timestamp = hrt_absolute_time();
|
current_status->timestamp = hrt_absolute_time();
|
||||||
|
@ -217,9 +224,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
|
||||||
/* assemble state vector based on flag values */
|
/* assemble state vector based on flag values */
|
||||||
if (current_status->flag_control_rates_enabled) {
|
if (current_status->flag_control_rates_enabled) {
|
||||||
current_status->onboard_control_sensors_present |= 0x400;
|
current_status->onboard_control_sensors_present |= 0x400;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
current_status->onboard_control_sensors_present &= ~0x400;
|
current_status->onboard_control_sensors_present &= ~0x400;
|
||||||
}
|
}
|
||||||
|
|
||||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
|
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
|
||||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
|
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
|
||||||
current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
|
current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
|
||||||
|
@ -232,10 +241,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
|
||||||
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
|
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
|
||||||
|
|
||||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||||
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
|
printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
|
||||||
}
|
}
|
||||||
|
|
||||||
void publish_armed_status(const struct vehicle_status_s *current_status) {
|
void publish_armed_status(const struct vehicle_status_s *current_status)
|
||||||
|
{
|
||||||
struct actuator_armed_s armed;
|
struct actuator_armed_s armed;
|
||||||
armed.armed = current_status->flag_system_armed;
|
armed.armed = current_status->flag_system_armed;
|
||||||
/* lock down actuators if required, only in HIL */
|
/* lock down actuators if required, only in HIL */
|
||||||
|
@ -250,19 +260,19 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
|
||||||
*/
|
*/
|
||||||
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
{
|
{
|
||||||
fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
|
warnx("EMERGENCY HANDLER\n");
|
||||||
/* Depending on the current state go to one of the error states */
|
/* Depending on the current state go to one of the error states */
|
||||||
|
|
||||||
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
|
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
|
||||||
|
|
||||||
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
|
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
|
||||||
|
|
||||||
// DO NOT abort mission
|
// DO NOT abort mission
|
||||||
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
|
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
|
warnx("Unknown system state: #%d\n", current_status->state_machine);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -272,7 +282,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
|
||||||
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
|
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
|
//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -497,7 +507,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
|
||||||
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
{
|
{
|
||||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
|
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
|
||||||
printf("[commander] arming\n");
|
printf("[cmd] arming\n");
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -505,11 +515,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
|
||||||
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
{
|
{
|
||||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||||
printf("[commander] going standby\n");
|
printf("[cmd] going standby\n");
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
||||||
|
|
||||||
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||||
printf("[commander] MISSION ABORT!\n");
|
printf("[cmd] MISSION ABORT!\n");
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -518,54 +528,139 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
|
||||||
{
|
{
|
||||||
int old_mode = current_status->flight_mode;
|
int old_mode = current_status->flight_mode;
|
||||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
|
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
|
||||||
|
|
||||||
current_status->flag_control_manual_enabled = true;
|
current_status->flag_control_manual_enabled = true;
|
||||||
/* enable attitude control per default */
|
|
||||||
current_status->flag_control_attitude_enabled = true;
|
/* set behaviour based on airframe */
|
||||||
current_status->flag_control_rates_enabled = true;
|
if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||||
|
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||||
|
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
|
||||||
|
|
||||||
|
/* assuming a rotary wing, set to SAS */
|
||||||
|
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
|
||||||
|
current_status->flag_control_attitude_enabled = true;
|
||||||
|
current_status->flag_control_rates_enabled = true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
/* assuming a fixed wing, set to direct pass-through */
|
||||||
|
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
|
||||||
|
current_status->flag_control_attitude_enabled = false;
|
||||||
|
current_status->flag_control_rates_enabled = false;
|
||||||
|
}
|
||||||
|
|
||||||
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||||
|
|
||||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||||
printf("[commander] manual mode\n");
|
printf("[cmd] manual mode\n");
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
{
|
{
|
||||||
int old_mode = current_status->flight_mode;
|
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
|
int old_mode = current_status->flight_mode;
|
||||||
current_status->flag_control_manual_enabled = true;
|
int old_manual_control_mode = current_status->manual_control_mode;
|
||||||
current_status->flag_control_attitude_enabled = true;
|
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
|
||||||
current_status->flag_control_rates_enabled = true;
|
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
|
||||||
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
current_status->flag_control_attitude_enabled = true;
|
||||||
|
current_status->flag_control_rates_enabled = true;
|
||||||
|
current_status->flag_control_manual_enabled = true;
|
||||||
|
|
||||||
|
if (old_mode != current_status->flight_mode ||
|
||||||
|
old_manual_control_mode != current_status->manual_control_mode) {
|
||||||
|
printf("[cmd] att stabilized mode\n");
|
||||||
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
|
||||||
|
state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
|
{
|
||||||
|
if (!current_status->flag_vector_flight_mode_ok) {
|
||||||
|
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
|
||||||
|
tune_error();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
|
||||||
printf("[commander] stabilized mode\n");
|
printf("[cmd] position guided mode\n");
|
||||||
|
int old_mode = current_status->flight_mode;
|
||||||
|
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
|
||||||
|
current_status->flag_control_manual_enabled = false;
|
||||||
|
current_status->flag_control_attitude_enabled = true;
|
||||||
|
current_status->flag_control_rates_enabled = true;
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
|
||||||
|
|
||||||
|
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||||
{
|
{
|
||||||
int old_mode = current_status->flight_mode;
|
if (!current_status->flag_vector_flight_mode_ok) {
|
||||||
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
|
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
|
||||||
current_status->flag_control_manual_enabled = true;
|
return;
|
||||||
current_status->flag_control_attitude_enabled = true;
|
}
|
||||||
current_status->flag_control_rates_enabled = true;
|
|
||||||
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
|
||||||
|
|
||||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
|
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
|
||||||
printf("[commander] auto mode\n");
|
printf("[cmd] auto mode\n");
|
||||||
|
int old_mode = current_status->flight_mode;
|
||||||
|
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
|
||||||
|
current_status->flag_control_manual_enabled = false;
|
||||||
|
current_status->flag_control_attitude_enabled = true;
|
||||||
|
current_status->flag_control_rates_enabled = true;
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
|
||||||
|
|
||||||
|
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
|
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
|
||||||
{
|
{
|
||||||
printf("[commander] Requested new mode: %d\n", (int)mode);
|
|
||||||
uint8_t ret = 1;
|
uint8_t ret = 1;
|
||||||
|
|
||||||
|
/* Switch on HIL if in standby and not already in HIL mode */
|
||||||
|
if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
|
||||||
|
&& !current_status->flag_hil_enabled) {
|
||||||
|
if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
|
||||||
|
/* Enable HIL on request */
|
||||||
|
current_status->flag_hil_enabled = true;
|
||||||
|
ret = OK;
|
||||||
|
state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||||
|
publish_armed_status(current_status);
|
||||||
|
printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
|
||||||
|
|
||||||
|
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
|
||||||
|
current_status->flag_system_armed) {
|
||||||
|
|
||||||
|
mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* switch manual / auto */
|
||||||
|
if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
|
||||||
|
update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
|
||||||
|
|
||||||
|
} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
|
||||||
|
update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
|
||||||
|
|
||||||
|
} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
|
||||||
|
update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
|
||||||
|
|
||||||
|
} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||||
|
update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
|
||||||
|
}
|
||||||
|
|
||||||
/* vehicle is disarmed, mode requests arming */
|
/* vehicle is disarmed, mode requests arming */
|
||||||
if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
|
if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
|
||||||
/* only arm in standby state */
|
/* only arm in standby state */
|
||||||
|
@ -573,7 +668,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
|
||||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
|
||||||
ret = OK;
|
ret = OK;
|
||||||
printf("[commander] arming due to command request\n");
|
printf("[cmd] arming due to command request\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -583,26 +678,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
|
||||||
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
|
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
|
||||||
ret = OK;
|
ret = OK;
|
||||||
printf("[commander] disarming due to command request\n");
|
printf("[cmd] disarming due to command request\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Switch on HIL if in standby and not already in HIL mode */
|
|
||||||
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
|
|
||||||
&& !current_status->flag_hil_enabled) {
|
|
||||||
/* Enable HIL on request */
|
|
||||||
current_status->flag_hil_enabled = true;
|
|
||||||
ret = OK;
|
|
||||||
state_machine_publish(status_pub, current_status, mavlink_fd);
|
|
||||||
publish_armed_status(current_status);
|
|
||||||
printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
|
|
||||||
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
|
|
||||||
mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
|
|
||||||
}
|
|
||||||
|
|
||||||
/* NEVER actually switch off HIL without reboot */
|
/* NEVER actually switch off HIL without reboot */
|
||||||
if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
|
if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
|
||||||
fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
|
warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
|
||||||
|
mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
|
||||||
ret = ERROR;
|
ret = ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -627,7 +710,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
|
||||||
|
|
||||||
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
|
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
|
||||||
printf("system will reboot\n");
|
printf("system will reboot\n");
|
||||||
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
|
mavlink_log_critical(mavlink_fd, "Rebooting..");
|
||||||
|
usleep(200000);
|
||||||
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
|
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
|
||||||
ret = 0;
|
ret = 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -127,6 +127,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
|
||||||
*/
|
*/
|
||||||
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Handle state machine if mode switch is guided
|
||||||
|
*
|
||||||
|
* @param status_pub file descriptor for state update topic publication
|
||||||
|
* @param current_status pointer to the current state machine to operate on
|
||||||
|
* @param mavlink_fd file descriptor for MAVLink statustext messages
|
||||||
|
*/
|
||||||
|
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Handle state machine if mode switch is auto
|
* Handle state machine if mode switch is auto
|
||||||
*
|
*
|
||||||
|
|
|
@ -0,0 +1,53 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Control library
|
||||||
|
#
|
||||||
|
CSRCS = test_params.c
|
||||||
|
|
||||||
|
CXXSRCS = block/Block.cpp \
|
||||||
|
block/BlockParam.cpp \
|
||||||
|
block/UOrbPublication.cpp \
|
||||||
|
block/UOrbSubscription.cpp \
|
||||||
|
blocks.cpp \
|
||||||
|
fixedwing.cpp
|
||||||
|
|
||||||
|
CXXHDRS = block/Block.hpp \
|
||||||
|
block/BlockParam.hpp \
|
||||||
|
block/UOrbPublication.hpp \
|
||||||
|
block/UOrbSubscription.hpp \
|
||||||
|
blocks.hpp \
|
||||||
|
fixedwing.hpp
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,210 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 Block.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "BlockParam.hpp"
|
||||||
|
#include "UOrbSubscription.hpp"
|
||||||
|
#include "UOrbPublication.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
Block::Block(SuperBlock *parent, const char *name) :
|
||||||
|
_name(name),
|
||||||
|
_parent(parent),
|
||||||
|
_dt(0),
|
||||||
|
_subscriptions(),
|
||||||
|
_params()
|
||||||
|
{
|
||||||
|
if (getParent() != NULL) {
|
||||||
|
getParent()->getChildren().add(this);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::getName(char *buf, size_t n)
|
||||||
|
{
|
||||||
|
if (getParent() == NULL) {
|
||||||
|
strncpy(buf, _name, n);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
char parentName[blockNameLengthMax];
|
||||||
|
getParent()->getName(parentName, n);
|
||||||
|
|
||||||
|
if (!strcmp(_name, "")) {
|
||||||
|
strncpy(buf, parentName, blockNameLengthMax);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::updateParams()
|
||||||
|
{
|
||||||
|
BlockParamBase *param = getParams().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (param != NULL) {
|
||||||
|
if (count++ > maxParamsPerBlock) {
|
||||||
|
char name[blockNameLengthMax];
|
||||||
|
getName(name, blockNameLengthMax);
|
||||||
|
printf("exceeded max params for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
//printf("updating param: %s\n", param->getName());
|
||||||
|
param->update();
|
||||||
|
param = param->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::updateSubscriptions()
|
||||||
|
{
|
||||||
|
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (sub != NULL) {
|
||||||
|
if (count++ > maxSubscriptionsPerBlock) {
|
||||||
|
char name[blockNameLengthMax];
|
||||||
|
getName(name, blockNameLengthMax);
|
||||||
|
printf("exceeded max subscriptions for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
sub->update();
|
||||||
|
sub = sub->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::updatePublications()
|
||||||
|
{
|
||||||
|
UOrbPublicationBase *pub = getPublications().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (pub != NULL) {
|
||||||
|
if (count++ > maxPublicationsPerBlock) {
|
||||||
|
char name[blockNameLengthMax];
|
||||||
|
getName(name, blockNameLengthMax);
|
||||||
|
printf("exceeded max publications for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub->update();
|
||||||
|
pub = pub->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::setDt(float dt)
|
||||||
|
{
|
||||||
|
Block::setDt(dt);
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->setDt(dt);
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::updateChildParams()
|
||||||
|
{
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->updateParams();
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::updateChildSubscriptions()
|
||||||
|
{
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->updateSubscriptions();
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::updateChildPublications()
|
||||||
|
{
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->updatePublications();
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,131 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 Block.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <inttypes.h>
|
||||||
|
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
static const uint16_t maxChildrenPerBlock = 100;
|
||||||
|
static const uint16_t maxParamsPerBlock = 100;
|
||||||
|
static const uint16_t maxSubscriptionsPerBlock = 100;
|
||||||
|
static const uint16_t maxPublicationsPerBlock = 100;
|
||||||
|
static const uint8_t blockNameLengthMax = 80;
|
||||||
|
|
||||||
|
// forward declaration
|
||||||
|
class BlockParamBase;
|
||||||
|
class UOrbSubscriptionBase;
|
||||||
|
class UOrbPublicationBase;
|
||||||
|
class SuperBlock;
|
||||||
|
|
||||||
|
/**
|
||||||
|
*/
|
||||||
|
class __EXPORT Block :
|
||||||
|
public ListNode<Block *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend class BlockParamBase;
|
||||||
|
// methods
|
||||||
|
Block(SuperBlock *parent, const char *name);
|
||||||
|
void getName(char *name, size_t n);
|
||||||
|
virtual ~Block() {};
|
||||||
|
virtual void updateParams();
|
||||||
|
virtual void updateSubscriptions();
|
||||||
|
virtual void updatePublications();
|
||||||
|
virtual void setDt(float dt) { _dt = dt; }
|
||||||
|
// accessors
|
||||||
|
float getDt() { return _dt; }
|
||||||
|
protected:
|
||||||
|
// accessors
|
||||||
|
SuperBlock *getParent() { return _parent; }
|
||||||
|
List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
|
||||||
|
List<UOrbPublicationBase *> & getPublications() { return _publications; }
|
||||||
|
List<BlockParamBase *> & getParams() { return _params; }
|
||||||
|
// attributes
|
||||||
|
const char *_name;
|
||||||
|
SuperBlock *_parent;
|
||||||
|
float _dt;
|
||||||
|
List<UOrbSubscriptionBase *> _subscriptions;
|
||||||
|
List<UOrbPublicationBase *> _publications;
|
||||||
|
List<BlockParamBase *> _params;
|
||||||
|
};
|
||||||
|
|
||||||
|
class __EXPORT SuperBlock :
|
||||||
|
public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend class Block;
|
||||||
|
// methods
|
||||||
|
SuperBlock(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_children() {
|
||||||
|
}
|
||||||
|
virtual ~SuperBlock() {};
|
||||||
|
virtual void setDt(float dt);
|
||||||
|
virtual void updateParams() {
|
||||||
|
Block::updateParams();
|
||||||
|
|
||||||
|
if (getChildren().getHead() != NULL) updateChildParams();
|
||||||
|
}
|
||||||
|
virtual void updateSubscriptions() {
|
||||||
|
Block::updateSubscriptions();
|
||||||
|
|
||||||
|
if (getChildren().getHead() != NULL) updateChildSubscriptions();
|
||||||
|
}
|
||||||
|
virtual void updatePublications() {
|
||||||
|
Block::updatePublications();
|
||||||
|
|
||||||
|
if (getChildren().getHead() != NULL) updateChildPublications();
|
||||||
|
}
|
||||||
|
protected:
|
||||||
|
// methods
|
||||||
|
List<Block *> & getChildren() { return _children; }
|
||||||
|
void updateChildParams();
|
||||||
|
void updateChildSubscriptions();
|
||||||
|
void updateChildPublications();
|
||||||
|
// attributes
|
||||||
|
List<Block *> _children;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,77 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 Blockparam.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "BlockParam.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
BlockParamBase::BlockParamBase(Block *parent, const char *name) :
|
||||||
|
_handle(PARAM_INVALID)
|
||||||
|
{
|
||||||
|
char fullname[blockNameLengthMax];
|
||||||
|
|
||||||
|
if (parent == NULL) {
|
||||||
|
strncpy(fullname, name, blockNameLengthMax);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
char parentName[blockNameLengthMax];
|
||||||
|
parent->getName(parentName, blockNameLengthMax);
|
||||||
|
|
||||||
|
if (!strcmp(name, "")) {
|
||||||
|
strncpy(fullname, parentName, blockNameLengthMax);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
|
||||||
|
}
|
||||||
|
|
||||||
|
parent->getParams().add(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
_handle = param_find(fullname);
|
||||||
|
|
||||||
|
if (_handle == PARAM_INVALID)
|
||||||
|
printf("error finding param: %s\n", fullname);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,85 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 BlockParam.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A base class for block params that enables traversing linked list.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
BlockParamBase(Block *parent, const char *name);
|
||||||
|
virtual ~BlockParamBase() {};
|
||||||
|
virtual void update() = 0;
|
||||||
|
const char *getName() { return param_name(_handle); }
|
||||||
|
protected:
|
||||||
|
param_t _handle;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Parameters that are tied to blocks for updating and nameing.
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT BlockParam : public BlockParamBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
BlockParam(Block *block, const char *name) :
|
||||||
|
BlockParamBase(block, name),
|
||||||
|
_val() {
|
||||||
|
update();
|
||||||
|
}
|
||||||
|
T get() { return _val; }
|
||||||
|
void set(T val) { _val = val; }
|
||||||
|
void update() {
|
||||||
|
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||||
|
}
|
||||||
|
protected:
|
||||||
|
T _val;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,71 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 Node.h
|
||||||
|
*
|
||||||
|
* A node of a linked list.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT ListNode
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ListNode() : _sibling(NULL) {
|
||||||
|
}
|
||||||
|
void setSibling(T sibling) { _sibling = sibling; }
|
||||||
|
T getSibling() { return _sibling; }
|
||||||
|
T get() {
|
||||||
|
return _sibling;
|
||||||
|
}
|
||||||
|
protected:
|
||||||
|
T _sibling;
|
||||||
|
};
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT List
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
List() : _head() {
|
||||||
|
}
|
||||||
|
void add(T newNode) {
|
||||||
|
newNode->setSibling(getHead());
|
||||||
|
setHead(newNode);
|
||||||
|
}
|
||||||
|
T getHead() { return _head; }
|
||||||
|
private:
|
||||||
|
void setHead(T &head) { _head = head; }
|
||||||
|
T _head;
|
||||||
|
};
|
|
@ -0,0 +1,39 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 UOrbPublication.cpp
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "UOrbPublication.hpp"
|
|
@ -0,0 +1,118 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 UOrbPublication.h
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
class Block;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Base publication warapper class, used in list traversal
|
||||||
|
* of various publications.
|
||||||
|
*/
|
||||||
|
class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
UOrbPublicationBase(
|
||||||
|
List<UOrbPublicationBase *> * list,
|
||||||
|
const struct orb_metadata *meta) :
|
||||||
|
_meta(meta),
|
||||||
|
_handle() {
|
||||||
|
if (list != NULL) list->add(this);
|
||||||
|
}
|
||||||
|
void update() {
|
||||||
|
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
|
||||||
|
}
|
||||||
|
virtual void *getDataVoidPtr() = 0;
|
||||||
|
virtual ~UOrbPublicationBase() {
|
||||||
|
orb_unsubscribe(getHandle());
|
||||||
|
}
|
||||||
|
const struct orb_metadata *getMeta() { return _meta; }
|
||||||
|
int getHandle() { return _handle; }
|
||||||
|
protected:
|
||||||
|
void setHandle(orb_advert_t handle) { _handle = handle; }
|
||||||
|
const struct orb_metadata *_meta;
|
||||||
|
orb_advert_t _handle;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* UOrb Publication wrapper class
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class UOrbPublication :
|
||||||
|
public T, // this must be first!
|
||||||
|
public UOrbPublicationBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
*
|
||||||
|
* @param list A list interface for adding to list during construction
|
||||||
|
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||||
|
* for the topic.
|
||||||
|
*/
|
||||||
|
UOrbPublication(
|
||||||
|
List<UOrbPublicationBase *> * list,
|
||||||
|
const struct orb_metadata *meta) :
|
||||||
|
T(), // initialize data structure to zero
|
||||||
|
UOrbPublicationBase(list, meta) {
|
||||||
|
// It is important that we call T()
|
||||||
|
// before we publish the data, so we
|
||||||
|
// call this here instead of the base class
|
||||||
|
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
|
||||||
|
}
|
||||||
|
virtual ~UOrbPublication() {}
|
||||||
|
/*
|
||||||
|
* XXX
|
||||||
|
* This function gets the T struct, assuming
|
||||||
|
* the struct is the first base class, this
|
||||||
|
* should use dynamic cast, but doesn't
|
||||||
|
* seem to be available
|
||||||
|
*/
|
||||||
|
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,51 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 UOrbSubscription.cpp
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "UOrbSubscription.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
bool __EXPORT UOrbSubscriptionBase::updated()
|
||||||
|
{
|
||||||
|
bool isUpdated = false;
|
||||||
|
orb_check(_handle, &isUpdated);
|
||||||
|
return isUpdated;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,137 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 UOrbSubscription.h
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
class Block;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Base subscription warapper class, used in list traversal
|
||||||
|
* of various subscriptions.
|
||||||
|
*/
|
||||||
|
class __EXPORT UOrbSubscriptionBase :
|
||||||
|
public ListNode<control::UOrbSubscriptionBase *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
*
|
||||||
|
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||||
|
* for the topic.
|
||||||
|
*/
|
||||||
|
UOrbSubscriptionBase(
|
||||||
|
List<UOrbSubscriptionBase *> * list,
|
||||||
|
const struct orb_metadata *meta) :
|
||||||
|
_meta(meta),
|
||||||
|
_handle() {
|
||||||
|
if (list != NULL) list->add(this);
|
||||||
|
}
|
||||||
|
bool updated();
|
||||||
|
void update() {
|
||||||
|
if (updated()) {
|
||||||
|
orb_copy(_meta, _handle, getDataVoidPtr());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
virtual void *getDataVoidPtr() = 0;
|
||||||
|
virtual ~UOrbSubscriptionBase() {
|
||||||
|
orb_unsubscribe(_handle);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
const struct orb_metadata *getMeta() { return _meta; }
|
||||||
|
int getHandle() { return _handle; }
|
||||||
|
protected:
|
||||||
|
// accessors
|
||||||
|
void setHandle(int handle) { _handle = handle; }
|
||||||
|
// attributes
|
||||||
|
const struct orb_metadata *_meta;
|
||||||
|
int _handle;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* UOrb Subscription wrapper class
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT UOrbSubscription :
|
||||||
|
public T, // this must be first!
|
||||||
|
public UOrbSubscriptionBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
*
|
||||||
|
* @param list A list interface for adding to list during construction
|
||||||
|
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||||
|
* for the topic.
|
||||||
|
* @param interval The minimum interval in milliseconds between updates
|
||||||
|
*/
|
||||||
|
UOrbSubscription(
|
||||||
|
List<UOrbSubscriptionBase *> * list,
|
||||||
|
const struct orb_metadata *meta, unsigned interval) :
|
||||||
|
T(), // initialize data structure to zero
|
||||||
|
UOrbSubscriptionBase(list, meta) {
|
||||||
|
setHandle(orb_subscribe(getMeta()));
|
||||||
|
orb_set_interval(getHandle(), interval);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deconstructor
|
||||||
|
*/
|
||||||
|
virtual ~UOrbSubscription() {}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* XXX
|
||||||
|
* This function gets the T struct, assuming
|
||||||
|
* the struct is the first base class, this
|
||||||
|
* should use dynamic cast, but doesn't
|
||||||
|
* seem to be available
|
||||||
|
*/
|
||||||
|
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||||
|
T getData() { return T(*this); }
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,486 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 blocks.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#include "blocks.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
int basicBlocksTest()
|
||||||
|
{
|
||||||
|
blockLimitTest();
|
||||||
|
blockLimitSymTest();
|
||||||
|
blockLowPassTest();
|
||||||
|
blockHighPassTest();
|
||||||
|
blockIntegralTest();
|
||||||
|
blockIntegralTrapTest();
|
||||||
|
blockDerivativeTest();
|
||||||
|
blockPTest();
|
||||||
|
blockPITest();
|
||||||
|
blockPDTest();
|
||||||
|
blockPIDTest();
|
||||||
|
blockOutputTest();
|
||||||
|
blockRandUniformTest();
|
||||||
|
blockRandGaussTest();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockLimit::update(float input)
|
||||||
|
{
|
||||||
|
if (input > getMax()) {
|
||||||
|
input = _max.get();
|
||||||
|
|
||||||
|
} else if (input < getMin()) {
|
||||||
|
input = getMin();
|
||||||
|
}
|
||||||
|
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockLimitTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockLimit\t\t\t: ");
|
||||||
|
BlockLimit limit(NULL, "TEST");
|
||||||
|
// initial state
|
||||||
|
ASSERT(equal(1.0f, limit.getMax()));
|
||||||
|
ASSERT(equal(-1.0f, limit.getMin()));
|
||||||
|
ASSERT(equal(0.0f, limit.getDt()));
|
||||||
|
// update
|
||||||
|
ASSERT(equal(-1.0f, limit.update(-2.0f)));
|
||||||
|
ASSERT(equal(1.0f, limit.update(2.0f)));
|
||||||
|
ASSERT(equal(0.0f, limit.update(0.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockLimitSym::update(float input)
|
||||||
|
{
|
||||||
|
if (input > getMax()) {
|
||||||
|
input = _max.get();
|
||||||
|
|
||||||
|
} else if (input < -getMax()) {
|
||||||
|
input = -getMax();
|
||||||
|
}
|
||||||
|
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockLimitSymTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockLimitSym\t\t: ");
|
||||||
|
BlockLimitSym limit(NULL, "TEST");
|
||||||
|
// initial state
|
||||||
|
ASSERT(equal(1.0f, limit.getMax()));
|
||||||
|
ASSERT(equal(0.0f, limit.getDt()));
|
||||||
|
// update
|
||||||
|
ASSERT(equal(-1.0f, limit.update(-2.0f)));
|
||||||
|
ASSERT(equal(1.0f, limit.update(2.0f)));
|
||||||
|
ASSERT(equal(0.0f, limit.update(0.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockLowPass::update(float input)
|
||||||
|
{
|
||||||
|
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||||
|
float a = b / (1 + b);
|
||||||
|
setState(a * input + (1 - a)*getState());
|
||||||
|
return getState();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockLowPassTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockLowPass\t\t: ");
|
||||||
|
BlockLowPass lowPass(NULL, "TEST_LP");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(10.0f, lowPass.getFCut()));
|
||||||
|
ASSERT(equal(0.0f, lowPass.getState()));
|
||||||
|
ASSERT(equal(0.0f, lowPass.getDt()));
|
||||||
|
// set dt
|
||||||
|
lowPass.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, lowPass.getDt()));
|
||||||
|
// set state
|
||||||
|
lowPass.setState(1.0f);
|
||||||
|
ASSERT(equal(1.0f, lowPass.getState()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
|
||||||
|
|
||||||
|
// test end condition
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
lowPass.update(2.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(2.0f, lowPass.getState()));
|
||||||
|
ASSERT(equal(2.0f, lowPass.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
float BlockHighPass::update(float input)
|
||||||
|
{
|
||||||
|
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||||
|
float a = 1 / (1 + b);
|
||||||
|
setY(a * (getY() + input - getU()));
|
||||||
|
setU(input);
|
||||||
|
return getY();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockHighPassTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockHighPass\t\t: ");
|
||||||
|
BlockHighPass highPass(NULL, "TEST_HP");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(10.0f, highPass.getFCut()));
|
||||||
|
ASSERT(equal(0.0f, highPass.getU()));
|
||||||
|
ASSERT(equal(0.0f, highPass.getY()));
|
||||||
|
ASSERT(equal(0.0f, highPass.getDt()));
|
||||||
|
// set dt
|
||||||
|
highPass.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, highPass.getDt()));
|
||||||
|
// set state
|
||||||
|
highPass.setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, highPass.getU()));
|
||||||
|
highPass.setY(1.0f);
|
||||||
|
ASSERT(equal(1.0f, highPass.getY()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(0.2746051f, highPass.update(2.0f)));
|
||||||
|
|
||||||
|
// test end condition
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
highPass.update(2.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(0.0f, highPass.getY()));
|
||||||
|
ASSERT(equal(0.0f, highPass.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockIntegral::update(float input)
|
||||||
|
{
|
||||||
|
// trapezoidal integration
|
||||||
|
setY(_limit.update(getY() + input * getDt()));
|
||||||
|
return getY();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockIntegralTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockIntegral\t\t: ");
|
||||||
|
BlockIntegral integral(NULL, "TEST_I");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(1.0f, integral.getMax()));
|
||||||
|
ASSERT(equal(0.0f, integral.getDt()));
|
||||||
|
// set dt
|
||||||
|
integral.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, integral.getDt()));
|
||||||
|
// set Y
|
||||||
|
integral.setY(0.9f);
|
||||||
|
ASSERT(equal(0.9f, integral.getY()));
|
||||||
|
|
||||||
|
// test exceed max
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(1.0f, integral.update(1.0f)));
|
||||||
|
// test exceed min
|
||||||
|
integral.setY(-0.9f);
|
||||||
|
ASSERT(equal(-0.9f, integral.getY()));
|
||||||
|
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(-1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(-1.0f, integral.update(-1.0f)));
|
||||||
|
// test update
|
||||||
|
integral.setY(0.1f);
|
||||||
|
ASSERT(equal(0.2f, integral.update(1.0)));
|
||||||
|
ASSERT(equal(0.2f, integral.getY()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockIntegralTrap::update(float input)
|
||||||
|
{
|
||||||
|
// trapezoidal integration
|
||||||
|
setY(_limit.update(getY() +
|
||||||
|
(getU() + input) / 2.0f * getDt()));
|
||||||
|
setU(input);
|
||||||
|
return getY();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockIntegralTrapTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockIntegralTrap\t\t: ");
|
||||||
|
BlockIntegralTrap integral(NULL, "TEST_I");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(1.0f, integral.getMax()));
|
||||||
|
ASSERT(equal(0.0f, integral.getDt()));
|
||||||
|
// set dt
|
||||||
|
integral.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, integral.getDt()));
|
||||||
|
// set U
|
||||||
|
integral.setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, integral.getU()));
|
||||||
|
// set Y
|
||||||
|
integral.setY(0.9f);
|
||||||
|
ASSERT(equal(0.9f, integral.getY()));
|
||||||
|
|
||||||
|
// test exceed max
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(1.0f, integral.update(1.0f)));
|
||||||
|
// test exceed min
|
||||||
|
integral.setU(-1.0f);
|
||||||
|
integral.setY(-0.9f);
|
||||||
|
ASSERT(equal(-0.9f, integral.getY()));
|
||||||
|
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(-1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(-1.0f, integral.update(-1.0f)));
|
||||||
|
// test update
|
||||||
|
integral.setU(2.0f);
|
||||||
|
integral.setY(0.1f);
|
||||||
|
ASSERT(equal(0.25f, integral.update(1.0)));
|
||||||
|
ASSERT(equal(0.25f, integral.getY()));
|
||||||
|
ASSERT(equal(1.0f, integral.getU()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockDerivative::update(float input)
|
||||||
|
{
|
||||||
|
float output = _lowPass.update((input - getU()) / getDt());
|
||||||
|
setU(input);
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockDerivativeTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockDerivative\t\t: ");
|
||||||
|
BlockDerivative derivative(NULL, "TEST_D");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, derivative.getU()));
|
||||||
|
ASSERT(equal(10.0f, derivative.getLP()));
|
||||||
|
// set dt
|
||||||
|
derivative.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, derivative.getDt()));
|
||||||
|
// set U
|
||||||
|
derivative.setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, derivative.getU()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(8.6269744f, derivative.update(2.0f)));
|
||||||
|
ASSERT(equal(2.0f, derivative.getU()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockP\t\t\t: ");
|
||||||
|
BlockP blockP(NULL, "TEST_P");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockP.getKP()));
|
||||||
|
ASSERT(equal(0.0f, blockP.getDt()));
|
||||||
|
// set dt
|
||||||
|
blockP.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockP.getDt()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(0.4f, blockP.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPITest()
|
||||||
|
{
|
||||||
|
printf("Test BlockPI\t\t\t: ");
|
||||||
|
BlockPI blockPI(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockPI.getKP()));
|
||||||
|
ASSERT(equal(0.1f, blockPI.getKI()));
|
||||||
|
ASSERT(equal(0.0f, blockPI.getDt()));
|
||||||
|
ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
|
||||||
|
// set dt
|
||||||
|
blockPI.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPI.getDt()));
|
||||||
|
// set integral state
|
||||||
|
blockPI.getIntegral().setY(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
|
||||||
|
// test update
|
||||||
|
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
|
||||||
|
ASSERT(equal(0.43f, blockPI.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPDTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockPD\t\t\t: ");
|
||||||
|
BlockPD blockPD(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockPD.getKP()));
|
||||||
|
ASSERT(equal(0.01f, blockPD.getKD()));
|
||||||
|
ASSERT(equal(0.0f, blockPD.getDt()));
|
||||||
|
ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
|
||||||
|
// set dt
|
||||||
|
blockPD.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPD.getDt()));
|
||||||
|
// set derivative state
|
||||||
|
blockPD.getDerivative().setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
|
||||||
|
// test update
|
||||||
|
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
|
||||||
|
ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPIDTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockPID\t\t\t: ");
|
||||||
|
BlockPID blockPID(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockPID.getKP()));
|
||||||
|
ASSERT(equal(0.1f, blockPID.getKI()));
|
||||||
|
ASSERT(equal(0.01f, blockPID.getKD()));
|
||||||
|
ASSERT(equal(0.0f, blockPID.getDt()));
|
||||||
|
ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
|
||||||
|
ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
|
||||||
|
// set dt
|
||||||
|
blockPID.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPID.getDt()));
|
||||||
|
// set derivative state
|
||||||
|
blockPID.getDerivative().setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
|
||||||
|
// set integral state
|
||||||
|
blockPID.getIntegral().setY(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
|
||||||
|
// test update
|
||||||
|
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
|
||||||
|
ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockOutputTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockOutput\t\t: ");
|
||||||
|
BlockOutput blockOutput(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, blockOutput.getDt()));
|
||||||
|
ASSERT(equal(0.5f, blockOutput.get()));
|
||||||
|
ASSERT(equal(-1.0f, blockOutput.getMin()));
|
||||||
|
ASSERT(equal(1.0f, blockOutput.getMax()));
|
||||||
|
// test update below min
|
||||||
|
blockOutput.update(-2.0f);
|
||||||
|
ASSERT(equal(-1.0f, blockOutput.get()));
|
||||||
|
// test update above max
|
||||||
|
blockOutput.update(2.0f);
|
||||||
|
ASSERT(equal(1.0f, blockOutput.get()));
|
||||||
|
// test trim
|
||||||
|
blockOutput.update(0.0f);
|
||||||
|
ASSERT(equal(0.5f, blockOutput.get()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockRandUniformTest()
|
||||||
|
{
|
||||||
|
srand(1234);
|
||||||
|
printf("Test BlockRandUniform\t\t: ");
|
||||||
|
BlockRandUniform blockRandUniform(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, blockRandUniform.getDt()));
|
||||||
|
ASSERT(equal(-1.0f, blockRandUniform.getMin()));
|
||||||
|
ASSERT(equal(1.0f, blockRandUniform.getMax()));
|
||||||
|
// test update
|
||||||
|
int n = 10000;
|
||||||
|
float mean = blockRandUniform.update();
|
||||||
|
|
||||||
|
// recursive mean algorithm from Knuth
|
||||||
|
for (int i = 2; i < n + 1; i++) {
|
||||||
|
float val = blockRandUniform.update();
|
||||||
|
mean += (val - mean) / i;
|
||||||
|
ASSERT(val <= blockRandUniform.getMax());
|
||||||
|
ASSERT(val >= blockRandUniform.getMin());
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(mean, (blockRandUniform.getMin() +
|
||||||
|
blockRandUniform.getMax()) / 2, 1e-1));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockRandGaussTest()
|
||||||
|
{
|
||||||
|
srand(1234);
|
||||||
|
printf("Test BlockRandGauss\t\t: ");
|
||||||
|
BlockRandGauss blockRandGauss(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, blockRandGauss.getDt()));
|
||||||
|
ASSERT(equal(1.0f, blockRandGauss.getMean()));
|
||||||
|
ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
|
||||||
|
// test update
|
||||||
|
int n = 10000;
|
||||||
|
float mean = blockRandGauss.update();
|
||||||
|
float sum = 0;
|
||||||
|
|
||||||
|
// recursive mean, stdev algorithm from Knuth
|
||||||
|
for (int i = 2; i < n + 1; i++) {
|
||||||
|
float val = blockRandGauss.update();
|
||||||
|
float newMean = mean + (val - mean) / i;
|
||||||
|
sum += (val - mean) * (val - newMean);
|
||||||
|
mean = newMean;
|
||||||
|
}
|
||||||
|
|
||||||
|
float stdDev = sqrt(sum / (n - 1));
|
||||||
|
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
|
||||||
|
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,494 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 blocks.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <assert.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <mathlib/math/test/test.hpp>
|
||||||
|
|
||||||
|
#include "block/Block.hpp"
|
||||||
|
#include "block/BlockParam.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
int __EXPORT basicBlocksTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A limiter/ saturation.
|
||||||
|
* The output of update is the input, bounded
|
||||||
|
* by min/max.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockLimit : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockLimit(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_min(this, "MIN"),
|
||||||
|
_max(this, "MAX")
|
||||||
|
{};
|
||||||
|
virtual ~BlockLimit() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getMin() { return _min.get(); }
|
||||||
|
float getMax() { return _max.get(); }
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _min;
|
||||||
|
BlockParam<float> _max;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockLimitTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A symmetric limiter/ saturation.
|
||||||
|
* Same as limiter but with only a max, is used for
|
||||||
|
* upper limit of +max, and lower limit of -max
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockLimitSym : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockLimitSym(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_max(this, "MAX")
|
||||||
|
{};
|
||||||
|
virtual ~BlockLimitSym() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getMax() { return _max.get(); }
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _max;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockLimitSymTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A low pass filter as described here:
|
||||||
|
* http://en.wikipedia.org/wiki/Low-pass_filter.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockLowPass : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockLowPass(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_state(0),
|
||||||
|
_fCut(this, "") // only one parameter, no need to name
|
||||||
|
{};
|
||||||
|
virtual ~BlockLowPass() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getState() { return _state; }
|
||||||
|
float getFCut() { return _fCut.get(); }
|
||||||
|
void setState(float state) { _state = state; }
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _state;
|
||||||
|
BlockParam<float> _fCut;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockLowPassTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A high pass filter as described here:
|
||||||
|
* http://en.wikipedia.org/wiki/High-pass_filter.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockHighPass : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockHighPass(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_u(0),
|
||||||
|
_y(0),
|
||||||
|
_fCut(this, "") // only one parameter, no need to name
|
||||||
|
{};
|
||||||
|
virtual ~BlockHighPass() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getU() {return _u;}
|
||||||
|
float getY() {return _y;}
|
||||||
|
float getFCut() {return _fCut.get();}
|
||||||
|
void setU(float u) {_u = u;}
|
||||||
|
void setY(float y) {_y = y;}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _u; /**< previous input */
|
||||||
|
float _y; /**< previous output */
|
||||||
|
BlockParam<float> _fCut; /**< cut-off frequency, Hz */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockHighPassTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A rectangular integrator.
|
||||||
|
* A limiter is built into the class to bound the
|
||||||
|
* integral's internal state. This is important
|
||||||
|
* for windup protection.
|
||||||
|
* @see Limit
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockIntegral: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockIntegral(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_y(0),
|
||||||
|
_limit(this, "") {};
|
||||||
|
virtual ~BlockIntegral() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getY() {return _y;}
|
||||||
|
float getMax() {return _limit.getMax();}
|
||||||
|
void setY(float y) {_y = y;}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _y; /**< previous output */
|
||||||
|
BlockLimitSym _limit; /**< limiter */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockIntegralTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A trapezoidal integrator.
|
||||||
|
* http://en.wikipedia.org/wiki/Trapezoidal_rule
|
||||||
|
* A limiter is built into the class to bound the
|
||||||
|
* integral's internal state. This is important
|
||||||
|
* for windup protection.
|
||||||
|
* @see Limit
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockIntegralTrap : public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockIntegralTrap(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_u(0),
|
||||||
|
_y(0),
|
||||||
|
_limit(this, "") {};
|
||||||
|
virtual ~BlockIntegralTrap() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getU() {return _u;}
|
||||||
|
float getY() {return _y;}
|
||||||
|
float getMax() {return _limit.getMax();}
|
||||||
|
void setU(float u) {_u = u;}
|
||||||
|
void setY(float y) {_y = y;}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _u; /**< previous input */
|
||||||
|
float _y; /**< previous output */
|
||||||
|
BlockLimitSym _limit; /**< limiter */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockIntegralTrapTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A simple derivative approximation.
|
||||||
|
* This uses the previous and current input.
|
||||||
|
* This has a built in low pass filter.
|
||||||
|
* @see LowPass
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockDerivative : public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockDerivative(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_u(0),
|
||||||
|
_lowPass(this, "LP")
|
||||||
|
{};
|
||||||
|
virtual ~BlockDerivative() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
void setU(float u) {_u = u;}
|
||||||
|
float getU() {return _u;}
|
||||||
|
float getLP() {return _lowPass.getFCut();}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _u; /**< previous input */
|
||||||
|
BlockLowPass _lowPass; /**< low pass filter */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockDerivativeTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockP: public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockP(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_kP(this, "") // only one param, no need to name
|
||||||
|
{};
|
||||||
|
virtual ~BlockP() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input;
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
protected:
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional-integral controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockPI: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockPI(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_integral(this, "I"),
|
||||||
|
_kP(this, "P"),
|
||||||
|
_kI(this, "I")
|
||||||
|
{};
|
||||||
|
virtual ~BlockPI() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input +
|
||||||
|
getKI() * getIntegral().update(input);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
float getKI() { return _kI.get(); }
|
||||||
|
BlockIntegral &getIntegral() { return _integral; }
|
||||||
|
private:
|
||||||
|
BlockIntegral _integral;
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
BlockParam<float> _kI;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPITest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional-derivative controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockPD: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockPD(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_derivative(this, "D"),
|
||||||
|
_kP(this, "P"),
|
||||||
|
_kD(this, "D")
|
||||||
|
{};
|
||||||
|
virtual ~BlockPD() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input +
|
||||||
|
getKD() * getDerivative().update(input);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
float getKD() { return _kD.get(); }
|
||||||
|
BlockDerivative &getDerivative() { return _derivative; }
|
||||||
|
private:
|
||||||
|
BlockDerivative _derivative;
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
BlockParam<float> _kD;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPDTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional-integral-derivative controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockPID: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockPID(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_integral(this, "I"),
|
||||||
|
_derivative(this, "D"),
|
||||||
|
_kP(this, "P"),
|
||||||
|
_kI(this, "I"),
|
||||||
|
_kD(this, "D")
|
||||||
|
{};
|
||||||
|
virtual ~BlockPID() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input +
|
||||||
|
getKI() * getIntegral().update(input) +
|
||||||
|
getKD() * getDerivative().update(input);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
float getKI() { return _kI.get(); }
|
||||||
|
float getKD() { return _kD.get(); }
|
||||||
|
BlockIntegral &getIntegral() { return _integral; }
|
||||||
|
BlockDerivative &getDerivative() { return _derivative; }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockIntegral _integral;
|
||||||
|
BlockDerivative _derivative;
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
BlockParam<float> _kI;
|
||||||
|
BlockParam<float> _kD;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPIDTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* An output trim/ saturation block
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockOutput: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockOutput(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_trim(this, "TRIM"),
|
||||||
|
_limit(this, ""),
|
||||||
|
_val(0) {
|
||||||
|
update(0);
|
||||||
|
};
|
||||||
|
virtual ~BlockOutput() {};
|
||||||
|
void update(float input) {
|
||||||
|
_val = _limit.update(input + getTrim());
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getMin() { return _limit.getMin(); }
|
||||||
|
float getMax() { return _limit.getMax(); }
|
||||||
|
float getTrim() { return _trim.get(); }
|
||||||
|
float get() { return _val; }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _trim;
|
||||||
|
BlockLimit _limit;
|
||||||
|
float _val;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockOutputTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A uniform random number generator
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockRandUniform: public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockRandUniform(SuperBlock *parent,
|
||||||
|
const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_min(this, "MIN"),
|
||||||
|
_max(this, "MAX") {
|
||||||
|
// seed should be initialized somewhere
|
||||||
|
// in main program for all calls to rand
|
||||||
|
// XXX currently in nuttx if you seed to 0, rand breaks
|
||||||
|
};
|
||||||
|
virtual ~BlockRandUniform() {};
|
||||||
|
float update() {
|
||||||
|
static float rand_max = MAX_RAND;
|
||||||
|
float rand_val = rand();
|
||||||
|
float bounds = getMax() - getMin();
|
||||||
|
return getMin() + (rand_val * bounds) / rand_max;
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getMin() { return _min.get(); }
|
||||||
|
float getMax() { return _max.get(); }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _min;
|
||||||
|
BlockParam<float> _max;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockRandUniformTest();
|
||||||
|
|
||||||
|
class __EXPORT BlockRandGauss: public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockRandGauss(SuperBlock *parent,
|
||||||
|
const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_mean(this, "MEAN"),
|
||||||
|
_stdDev(this, "DEV") {
|
||||||
|
// seed should be initialized somewhere
|
||||||
|
// in main program for all calls to rand
|
||||||
|
// XXX currently in nuttx if you seed to 0, rand breaks
|
||||||
|
};
|
||||||
|
virtual ~BlockRandGauss() {};
|
||||||
|
float update() {
|
||||||
|
static float V1, V2, S;
|
||||||
|
static int phase = 0;
|
||||||
|
float X;
|
||||||
|
|
||||||
|
if (phase == 0) {
|
||||||
|
do {
|
||||||
|
float U1 = (float)rand() / MAX_RAND;
|
||||||
|
float U2 = (float)rand() / MAX_RAND;
|
||||||
|
V1 = 2 * U1 - 1;
|
||||||
|
V2 = 2 * U2 - 1;
|
||||||
|
S = V1 * V1 + V2 * V2;
|
||||||
|
} while (S >= 1 || fabsf(S) < 1e-8f);
|
||||||
|
|
||||||
|
X = V1 * float(sqrt(-2 * float(log(S)) / S));
|
||||||
|
|
||||||
|
} else
|
||||||
|
X = V2 * float(sqrt(-2 * float(log(S)) / S));
|
||||||
|
|
||||||
|
phase = 1 - phase;
|
||||||
|
return X * getStdDev() + getMean();
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getMean() { return _mean.get(); }
|
||||||
|
float getStdDev() { return _stdDev.get(); }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _mean;
|
||||||
|
BlockParam<float> _stdDev;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockRandGaussTest();
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,367 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 fixedwing.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "fixedwing.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
namespace fixedwing
|
||||||
|
{
|
||||||
|
|
||||||
|
BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_rLowPass(this, "R_LP"),
|
||||||
|
_rWashout(this, "R_HP"),
|
||||||
|
_r2Rdr(this, "R2RDR"),
|
||||||
|
_rudder(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockYawDamper::~BlockYawDamper() {};
|
||||||
|
|
||||||
|
void BlockYawDamper::update(float rCmd, float r)
|
||||||
|
{
|
||||||
|
_rudder = _r2Rdr.update(rCmd -
|
||||||
|
_rWashout.update(_rLowPass.update(r)));
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_yawDamper(this, ""),
|
||||||
|
_pLowPass(this, "P_LP"),
|
||||||
|
_qLowPass(this, "Q_LP"),
|
||||||
|
_p2Ail(this, "P2AIL"),
|
||||||
|
_q2Elv(this, "Q2ELV"),
|
||||||
|
_aileron(0),
|
||||||
|
_elevator(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockStabilization::~BlockStabilization() {};
|
||||||
|
|
||||||
|
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
|
||||||
|
float p, float q, float r)
|
||||||
|
{
|
||||||
|
_aileron = _p2Ail.update(
|
||||||
|
pCmd - _pLowPass.update(p));
|
||||||
|
_elevator = _q2Elv.update(
|
||||||
|
qCmd - _qLowPass.update(q));
|
||||||
|
_yawDamper.update(rCmd, r);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_psi2Phi(this, "PSI2PHI"),
|
||||||
|
_phi2P(this, "PHI2P"),
|
||||||
|
_phiLimit(this, "PHI_LIM")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockHeadingHold::~BlockHeadingHold() {};
|
||||||
|
|
||||||
|
float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
|
||||||
|
{
|
||||||
|
float psiError = _wrap_pi(psiCmd - psi);
|
||||||
|
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
|
||||||
|
return _phi2P.update(phiCmd - phi);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_v2Theta(this, "V2THE"),
|
||||||
|
_theta2Q(this, "THE2Q"),
|
||||||
|
_theLimit(this, "THE"),
|
||||||
|
_vLimit(this, "V")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
|
||||||
|
|
||||||
|
float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
|
||||||
|
{
|
||||||
|
// negative sign because nose over to increase speed
|
||||||
|
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
|
||||||
|
return _theta2Q.update(thetaCmd - theta);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_v2Thr(this, "V2THR")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
|
||||||
|
|
||||||
|
float BlockVelocityHoldFrontside::update(float vCmd, float v)
|
||||||
|
{
|
||||||
|
return _v2Thr.update(vCmd - v);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_h2Thr(this, "H2THR"),
|
||||||
|
_throttle(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
|
||||||
|
|
||||||
|
void BlockAltitudeHoldBackside::update(float hCmd, float h)
|
||||||
|
{
|
||||||
|
_throttle = _h2Thr.update(hCmd - h);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_h2Theta(this, "H2THE"),
|
||||||
|
_theta2Q(this, "THE2Q")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
|
||||||
|
|
||||||
|
float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
|
||||||
|
{
|
||||||
|
float thetaCmd = _h2Theta.update(hCmd - h);
|
||||||
|
return _theta2Q.update(thetaCmd - theta);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
|
||||||
|
const char *name,
|
||||||
|
BlockStabilization *stabilization) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_stabilization(stabilization),
|
||||||
|
_headingHold(this, ""),
|
||||||
|
_velocityHold(this, ""),
|
||||||
|
_altitudeHold(this, ""),
|
||||||
|
_trimAil(this, "TRIM_AIL"),
|
||||||
|
_trimElv(this, "TRIM_ELV"),
|
||||||
|
_trimRdr(this, "TRIM_RDR"),
|
||||||
|
_trimThr(this, "TRIM_THR")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
|
||||||
|
|
||||||
|
void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
|
||||||
|
float h, float v,
|
||||||
|
float phi, float theta, float psi,
|
||||||
|
float p, float q, float r)
|
||||||
|
{
|
||||||
|
_altitudeHold.update(hCmd, h);
|
||||||
|
_stabilization->update(
|
||||||
|
_headingHold.update(psiCmd, phi, psi, p),
|
||||||
|
_velocityHold.update(vCmd, v, theta, q),
|
||||||
|
rCmd,
|
||||||
|
p, q, r);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_xtYawLimit(this, "XT2YAW"),
|
||||||
|
_xt2Yaw(this, "XT2YAW"),
|
||||||
|
_psiCmd(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockWaypointGuidance::~BlockWaypointGuidance() {};
|
||||||
|
|
||||||
|
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
|
||||||
|
vehicle_attitude_s &att,
|
||||||
|
vehicle_global_position_setpoint_s &posCmd,
|
||||||
|
vehicle_global_position_setpoint_s &lastPosCmd)
|
||||||
|
{
|
||||||
|
|
||||||
|
// heading to waypoint
|
||||||
|
float psiTrack = get_bearing_to_next_waypoint(
|
||||||
|
(double)pos.lat / (double)1e7d,
|
||||||
|
(double)pos.lon / (double)1e7d,
|
||||||
|
(double)posCmd.lat / (double)1e7d,
|
||||||
|
(double)posCmd.lon / (double)1e7d);
|
||||||
|
|
||||||
|
// cross track
|
||||||
|
struct crosstrack_error_s xtrackError;
|
||||||
|
get_distance_to_line(&xtrackError,
|
||||||
|
(double)pos.lat / (double)1e7d,
|
||||||
|
(double)pos.lon / (double)1e7d,
|
||||||
|
(double)lastPosCmd.lat / (double)1e7d,
|
||||||
|
(double)lastPosCmd.lon / (double)1e7d,
|
||||||
|
(double)posCmd.lat / (double)1e7d,
|
||||||
|
(double)posCmd.lon / (double)1e7d);
|
||||||
|
|
||||||
|
_psiCmd = _wrap_2pi(psiTrack -
|
||||||
|
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
// subscriptions
|
||||||
|
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
|
||||||
|
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
|
||||||
|
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
|
||||||
|
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
|
||||||
|
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
|
||||||
|
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
|
||||||
|
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
|
||||||
|
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||||
|
// publications
|
||||||
|
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
|
||||||
|
|
||||||
|
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
|
||||||
|
BlockUorbEnabledAutopilot(parent, name),
|
||||||
|
_stabilization(this, ""), // no name needed, already unique
|
||||||
|
_backsideAutopilot(this, "", &_stabilization),
|
||||||
|
_guide(this, ""),
|
||||||
|
_vCmd(this, "V_CMD"),
|
||||||
|
_attPoll(),
|
||||||
|
_lastPosCmd(),
|
||||||
|
_timeStamp(0)
|
||||||
|
{
|
||||||
|
_attPoll.fd = _att.getHandle();
|
||||||
|
_attPoll.events = POLLIN;
|
||||||
|
}
|
||||||
|
|
||||||
|
void BlockMultiModeBacksideAutopilot::update()
|
||||||
|
{
|
||||||
|
// wait for a sensor update, check for exit condition every 100 ms
|
||||||
|
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
|
||||||
|
|
||||||
|
uint64_t newTimeStamp = hrt_absolute_time();
|
||||||
|
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
|
||||||
|
_timeStamp = newTimeStamp;
|
||||||
|
|
||||||
|
// check for sane values of dt
|
||||||
|
// to prevent large control responses
|
||||||
|
if (dt > 1.0f || dt < 0) return;
|
||||||
|
|
||||||
|
// set dt for all child blocks
|
||||||
|
setDt(dt);
|
||||||
|
|
||||||
|
// store old position command before update if new command sent
|
||||||
|
if (_posCmd.updated()) {
|
||||||
|
_lastPosCmd = _posCmd.getData();
|
||||||
|
}
|
||||||
|
|
||||||
|
// check for new updates
|
||||||
|
if (_param_update.updated()) updateParams();
|
||||||
|
|
||||||
|
// get new information from subscriptions
|
||||||
|
updateSubscriptions();
|
||||||
|
|
||||||
|
// default all output to zero unless handled by mode
|
||||||
|
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||||
|
_actuators.control[i] = 0.0f;
|
||||||
|
|
||||||
|
// only update guidance in auto mode
|
||||||
|
if (_status.state_machine == SYSTEM_STATE_AUTO) {
|
||||||
|
// update guidance
|
||||||
|
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
// XXX handle STABILIZED (loiter on spot) as well
|
||||||
|
// once the system switches from manual or auto to stabilized
|
||||||
|
// the setpoint should update to loitering around this position
|
||||||
|
|
||||||
|
// handle autopilot modes
|
||||||
|
if (_status.state_machine == SYSTEM_STATE_AUTO ||
|
||||||
|
_status.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||||
|
|
||||||
|
// update guidance
|
||||||
|
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
|
||||||
|
|
||||||
|
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
||||||
|
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
|
||||||
|
|
||||||
|
// commands
|
||||||
|
float rCmd = 0;
|
||||||
|
|
||||||
|
_backsideAutopilot.update(
|
||||||
|
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
|
||||||
|
_pos.alt, v,
|
||||||
|
_att.roll, _att.pitch, _att.yaw,
|
||||||
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed
|
||||||
|
);
|
||||||
|
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
|
||||||
|
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
|
||||||
|
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
|
||||||
|
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
|
||||||
|
|
||||||
|
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
|
||||||
|
|
||||||
|
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||||
|
|
||||||
|
_actuators.control[CH_AIL] = _manual.roll;
|
||||||
|
_actuators.control[CH_ELV] = _manual.pitch;
|
||||||
|
_actuators.control[CH_RDR] = _manual.yaw;
|
||||||
|
_actuators.control[CH_THR] = _manual.throttle;
|
||||||
|
|
||||||
|
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||||
|
_stabilization.update(
|
||||||
|
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
|
||||||
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
||||||
|
_actuators.control[CH_AIL] = _stabilization.getAileron();
|
||||||
|
_actuators.control[CH_ELV] = _stabilization.getElevator();
|
||||||
|
_actuators.control[CH_RDR] = _stabilization.getRudder();
|
||||||
|
_actuators.control[CH_THR] = _manual.throttle;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// update all publications
|
||||||
|
updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
|
||||||
|
{
|
||||||
|
// send one last publication when destroyed, setting
|
||||||
|
// all output to zero
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||||
|
_actuators.control[i] = 0.0f;
|
||||||
|
|
||||||
|
updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace fixedwing
|
||||||
|
|
||||||
|
} // namespace control
|
||||||
|
|
|
@ -0,0 +1,438 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 fixedwing.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||||
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <poll.h>
|
||||||
|
|
||||||
|
#include "blocks.hpp"
|
||||||
|
#include "block/UOrbSubscription.hpp"
|
||||||
|
#include "block/UOrbPublication.hpp"
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
#include <systemlib/geo/geo.h>
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
namespace fixedwing
|
||||||
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* BlockYawDamper
|
||||||
|
*
|
||||||
|
* This block has more explations to help new developers
|
||||||
|
* add their own blocks. It includes a limited explanation
|
||||||
|
* of some C++ basics.
|
||||||
|
*
|
||||||
|
* Block: The generic class describing a typical block as you
|
||||||
|
* would expect in Simulink or ScicosLab. A block can have
|
||||||
|
* parameters. It cannot have other blocks.
|
||||||
|
*
|
||||||
|
* SuperBlock: A superblock is a block that can have other
|
||||||
|
* blocks. It has methods that manage the blocks beneath it.
|
||||||
|
*
|
||||||
|
* BlockYawDamper inherits from SuperBlock publically, this
|
||||||
|
* means that any public function in SuperBlock are public within
|
||||||
|
* BlockYawDamper and may be called from outside the
|
||||||
|
* class methods. Any protected function within block
|
||||||
|
* are private to the class and may not be called from
|
||||||
|
* outside this class. Protected should be preferred
|
||||||
|
* where possible to public as it is important to
|
||||||
|
* limit access to the bare minimum to prevent
|
||||||
|
* accidental errors.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockYawDamper : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
/**
|
||||||
|
* Declaring other blocks used by this block
|
||||||
|
*
|
||||||
|
* In this section we declare all child blocks that
|
||||||
|
* this block is composed of. They are private
|
||||||
|
* members so only this block has direct access to
|
||||||
|
* them.
|
||||||
|
*/
|
||||||
|
BlockLowPass _rLowPass;
|
||||||
|
BlockHighPass _rWashout;
|
||||||
|
BlockP _r2Rdr;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Declaring output values and accessors
|
||||||
|
*
|
||||||
|
* If we have any output values for the block we
|
||||||
|
* declare them here. Output can be directly returned
|
||||||
|
* through the update function, but outputs should be
|
||||||
|
* declared here if the information will likely be requested
|
||||||
|
* again, or if there are multiple outputs.
|
||||||
|
*
|
||||||
|
* You should only be able to set outputs from this block,
|
||||||
|
* so the outputs are declared in the private section.
|
||||||
|
* A get accessor is provided
|
||||||
|
* in the public section for other blocks to get the
|
||||||
|
* value of the output.
|
||||||
|
*/
|
||||||
|
float _rudder;
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* BlockYawDamper Constructor
|
||||||
|
*
|
||||||
|
* The job of the constructor is to initialize all
|
||||||
|
* parameter in this block and initialize all child
|
||||||
|
* blocks. Note also, that the output values must be
|
||||||
|
* initialized here. The order of the members in the
|
||||||
|
* member initialization list should follow the
|
||||||
|
* order in which they are declared within the class.
|
||||||
|
* See the private member declarations above.
|
||||||
|
*
|
||||||
|
* Block Construction
|
||||||
|
*
|
||||||
|
* All blocks are constructed with their parent block
|
||||||
|
* and their name. This allows parameters within the
|
||||||
|
* block to construct a fully qualified name from
|
||||||
|
* concatenating the two. If the name provided to the
|
||||||
|
* block is "", then the block will use the parent
|
||||||
|
* name as it's name. This is useful in cases where
|
||||||
|
* you have a block that has parameters "MIN", "MAX",
|
||||||
|
* such as BlockLimit and you do not want an extra name
|
||||||
|
* to qualify them since the parent block has no "MIN",
|
||||||
|
* "MAX" parameters.
|
||||||
|
*
|
||||||
|
* Block Parameter Construction
|
||||||
|
*
|
||||||
|
* Block parameters are named constants, they are
|
||||||
|
* constructed using:
|
||||||
|
* BlockParam::BlockParam(Block * parent, const char * name)
|
||||||
|
* This funciton takes both a parent block and a name.
|
||||||
|
* The constructore then uses the parent name and the name of
|
||||||
|
* the paramter to ask the px4 param library if it has any
|
||||||
|
* parameters with this name. If it does, a handle to the
|
||||||
|
* parameter is retrieved.
|
||||||
|
*
|
||||||
|
* Block/ BlockParam Naming
|
||||||
|
*
|
||||||
|
* When desigining new blocks, the naming of the parameters and
|
||||||
|
* blocks determines the fully qualified name of the parameters
|
||||||
|
* within the ground station, so it is important to choose
|
||||||
|
* short, easily understandable names. Again, when a name of
|
||||||
|
* "" is passed, the parent block name is used as the value to
|
||||||
|
* prepend to paramters names.
|
||||||
|
*/
|
||||||
|
BlockYawDamper(SuperBlock *parent, const char *name);
|
||||||
|
/**
|
||||||
|
* Block deconstructor
|
||||||
|
*
|
||||||
|
* It is always a good idea to declare a virtual
|
||||||
|
* deconstructor so that upon calling delete from
|
||||||
|
* a class derived from this, all of the
|
||||||
|
* deconstructors all called, the derived class first, and
|
||||||
|
* then the base class
|
||||||
|
*/
|
||||||
|
virtual ~BlockYawDamper();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Block update function
|
||||||
|
*
|
||||||
|
* The job of the update function is to compute the output
|
||||||
|
* values for the block. In a simple block with one output,
|
||||||
|
* the output may be returned directly. If the output is
|
||||||
|
* required frequenly by other processses, it might be a
|
||||||
|
* good idea to declare a member to store the temporary
|
||||||
|
* variable.
|
||||||
|
*/
|
||||||
|
void update(float rCmd, float r);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Rudder output value accessor
|
||||||
|
*
|
||||||
|
* This is a public accessor function, which means that the
|
||||||
|
* private value _rudder is returned to anyone calling
|
||||||
|
* BlockYawDamper::getRudder(). Note thate a setRudder() is
|
||||||
|
* not provided, this is because the updateParams() call
|
||||||
|
* for a block provides the mechanism for updating the
|
||||||
|
* paramter.
|
||||||
|
*/
|
||||||
|
float getRudder() { return _rudder; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Stability augmentation system.
|
||||||
|
* Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockStabilization : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockYawDamper _yawDamper;
|
||||||
|
BlockLowPass _pLowPass;
|
||||||
|
BlockLowPass _qLowPass;
|
||||||
|
BlockP _p2Ail;
|
||||||
|
BlockP _q2Elv;
|
||||||
|
float _aileron;
|
||||||
|
float _elevator;
|
||||||
|
public:
|
||||||
|
BlockStabilization(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockStabilization();
|
||||||
|
void update(float pCmd, float qCmd, float rCmd,
|
||||||
|
float p, float q, float r);
|
||||||
|
float getAileron() { return _aileron; }
|
||||||
|
float getElevator() { return _elevator; }
|
||||||
|
float getRudder() { return _yawDamper.getRudder(); }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Heading hold autopilot block.
|
||||||
|
* Aircraft Control and Simulation, Stevens and Lewis
|
||||||
|
* Heading hold, pg. 348
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockHeadingHold : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockP _psi2Phi;
|
||||||
|
BlockP _phi2P;
|
||||||
|
BlockLimitSym _phiLimit;
|
||||||
|
public:
|
||||||
|
BlockHeadingHold(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockHeadingHold();
|
||||||
|
/**
|
||||||
|
* returns pCmd
|
||||||
|
*/
|
||||||
|
float update(float psiCmd, float phi, float psi, float p);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Frontside/ Backside Control Systems
|
||||||
|
*
|
||||||
|
* Frontside :
|
||||||
|
* velocity error -> throttle
|
||||||
|
* altitude error -> elevator
|
||||||
|
*
|
||||||
|
* Backside :
|
||||||
|
* velocity error -> elevator
|
||||||
|
* altitude error -> throttle
|
||||||
|
*
|
||||||
|
* Backside control systems are more resilient at
|
||||||
|
* slow speeds on the back-side of the power
|
||||||
|
* required curve/ landing etc. Less performance
|
||||||
|
* than frontside at high speeds.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Backside velocity hold autopilot block.
|
||||||
|
* v -> theta -> q -> elevator
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockVelocityHoldBackside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _v2Theta;
|
||||||
|
BlockPID _theta2Q;
|
||||||
|
BlockLimit _theLimit;
|
||||||
|
BlockLimit _vLimit;
|
||||||
|
public:
|
||||||
|
BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockVelocityHoldBackside();
|
||||||
|
/**
|
||||||
|
* returns qCmd
|
||||||
|
*/
|
||||||
|
float update(float vCmd, float v, float theta, float q);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Frontside velocity hold autopilot block.
|
||||||
|
* v -> throttle
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _v2Thr;
|
||||||
|
public:
|
||||||
|
BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockVelocityHoldFrontside();
|
||||||
|
/**
|
||||||
|
* returns throttle
|
||||||
|
*/
|
||||||
|
float update(float vCmd, float v);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Backside altitude hold autopilot block.
|
||||||
|
* h -> throttle
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _h2Thr;
|
||||||
|
float _throttle;
|
||||||
|
public:
|
||||||
|
BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockAltitudeHoldBackside();
|
||||||
|
void update(float hCmd, float h);
|
||||||
|
float getThrottle() { return _throttle; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Frontside altitude hold autopilot block.
|
||||||
|
* h -> theta > q -> elevator
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _h2Theta;
|
||||||
|
BlockPID _theta2Q;
|
||||||
|
public:
|
||||||
|
BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockAltitudeHoldFrontside();
|
||||||
|
/**
|
||||||
|
* return qCmd
|
||||||
|
*/
|
||||||
|
float update(float hCmd, float h, float theta, float q);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Backside autopilot
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockBacksideAutopilot : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockStabilization *_stabilization;
|
||||||
|
BlockHeadingHold _headingHold;
|
||||||
|
BlockVelocityHoldBackside _velocityHold;
|
||||||
|
BlockAltitudeHoldBackside _altitudeHold;
|
||||||
|
BlockParam<float> _trimAil;
|
||||||
|
BlockParam<float> _trimElv;
|
||||||
|
BlockParam<float> _trimRdr;
|
||||||
|
BlockParam<float> _trimThr;
|
||||||
|
public:
|
||||||
|
BlockBacksideAutopilot(SuperBlock *parent,
|
||||||
|
const char *name,
|
||||||
|
BlockStabilization *stabilization);
|
||||||
|
virtual ~BlockBacksideAutopilot();
|
||||||
|
void update(float hCmd, float vCmd, float rCmd, float psiCmd,
|
||||||
|
float h, float v,
|
||||||
|
float phi, float theta, float psi,
|
||||||
|
float p, float q, float r);
|
||||||
|
float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
|
||||||
|
float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
|
||||||
|
float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
|
||||||
|
float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Waypoint Guidance block
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockWaypointGuidance : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockLimitSym _xtYawLimit;
|
||||||
|
BlockP _xt2Yaw;
|
||||||
|
float _psiCmd;
|
||||||
|
public:
|
||||||
|
BlockWaypointGuidance(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockWaypointGuidance();
|
||||||
|
void update(vehicle_global_position_s &pos,
|
||||||
|
vehicle_attitude_s &att,
|
||||||
|
vehicle_global_position_setpoint_s &posCmd,
|
||||||
|
vehicle_global_position_setpoint_s &lastPosCmd);
|
||||||
|
float getPsiCmd() { return _psiCmd; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* UorbEnabledAutopilot
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
// subscriptions
|
||||||
|
UOrbSubscription<vehicle_attitude_s> _att;
|
||||||
|
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||||
|
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||||
|
UOrbSubscription<vehicle_global_position_s> _pos;
|
||||||
|
UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
|
||||||
|
UOrbSubscription<manual_control_setpoint_s> _manual;
|
||||||
|
UOrbSubscription<vehicle_status_s> _status;
|
||||||
|
UOrbSubscription<parameter_update_s> _param_update;
|
||||||
|
// publications
|
||||||
|
UOrbPublication<actuator_controls_s> _actuators;
|
||||||
|
public:
|
||||||
|
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockUorbEnabledAutopilot();
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Multi-mode Autopilot
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockStabilization _stabilization;
|
||||||
|
BlockBacksideAutopilot _backsideAutopilot;
|
||||||
|
BlockWaypointGuidance _guide;
|
||||||
|
BlockParam<float> _vCmd;
|
||||||
|
|
||||||
|
struct pollfd _attPoll;
|
||||||
|
vehicle_global_position_setpoint_s _lastPosCmd;
|
||||||
|
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
|
||||||
|
uint64_t _timeStamp;
|
||||||
|
public:
|
||||||
|
BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
|
||||||
|
void update();
|
||||||
|
virtual ~BlockMultiModeBacksideAutopilot();
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace fixedwing
|
||||||
|
|
||||||
|
} // namespace control
|
||||||
|
|
|
@ -0,0 +1,22 @@
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
// WARNING:
|
||||||
|
// do not changes these unless
|
||||||
|
// you want to recompute the
|
||||||
|
// answers for all of the unit tests
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);
|
|
@ -95,8 +95,6 @@
|
||||||
* Protected Functions
|
* Protected Functions
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
extern int adc_devinit(void);
|
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Public Functions
|
* Public Functions
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
@ -150,9 +148,7 @@ __EXPORT int nsh_archinitialize(void)
|
||||||
int result;
|
int result;
|
||||||
|
|
||||||
/* configure the high-resolution time/callout interface */
|
/* configure the high-resolution time/callout interface */
|
||||||
#ifdef CONFIG_HRT_TIMER
|
|
||||||
hrt_init();
|
hrt_init();
|
||||||
#endif
|
|
||||||
|
|
||||||
/* configure CPU load estimation */
|
/* configure CPU load estimation */
|
||||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
||||||
|
@ -160,27 +156,21 @@ __EXPORT int nsh_archinitialize(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* set up the serial DMA polling */
|
/* set up the serial DMA polling */
|
||||||
#ifdef SERIAL_HAVE_DMA
|
static struct hrt_call serial_dma_call;
|
||||||
{
|
struct timespec ts;
|
||||||
static struct hrt_call serial_dma_call;
|
|
||||||
struct timespec ts;
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Poll at 1ms intervals for received bytes that have not triggered
|
* Poll at 1ms intervals for received bytes that have not triggered
|
||||||
* a DMA event.
|
* a DMA event.
|
||||||
*/
|
*/
|
||||||
ts.tv_sec = 0;
|
ts.tv_sec = 0;
|
||||||
ts.tv_nsec = 1000000;
|
ts.tv_nsec = 1000000;
|
||||||
|
|
||||||
hrt_call_every(&serial_dma_call,
|
hrt_call_every(&serial_dma_call,
|
||||||
ts_to_abstime(&ts),
|
ts_to_abstime(&ts),
|
||||||
ts_to_abstime(&ts),
|
ts_to_abstime(&ts),
|
||||||
(hrt_callout)stm32_serial_dma_poll,
|
(hrt_callout)stm32_serial_dma_poll,
|
||||||
NULL);
|
NULL);
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
message("\r\n");
|
|
||||||
|
|
||||||
// initial LED state
|
// initial LED state
|
||||||
drv_led_start();
|
drv_led_start();
|
||||||
|
@ -209,8 +199,7 @@ __EXPORT int nsh_archinitialize(void)
|
||||||
|
|
||||||
message("[boot] Successfully initialized SPI port 1\r\n");
|
message("[boot] Successfully initialized SPI port 1\r\n");
|
||||||
|
|
||||||
#if defined(CONFIG_STM32_SPI3)
|
/* Get the SPI port for the microSD slot */
|
||||||
/* Get the SPI port */
|
|
||||||
|
|
||||||
message("[boot] Initializing SPI port 3\n");
|
message("[boot] Initializing SPI port 3\n");
|
||||||
spi3 = up_spiinitialize(3);
|
spi3 = up_spiinitialize(3);
|
||||||
|
@ -233,23 +222,11 @@ __EXPORT int nsh_archinitialize(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
||||||
#endif /* SPI3 */
|
|
||||||
|
|
||||||
#ifdef CONFIG_ADC
|
stm32_configgpio(GPIO_ADC1_IN10);
|
||||||
int adc_state = adc_devinit();
|
stm32_configgpio(GPIO_ADC1_IN11);
|
||||||
|
stm32_configgpio(GPIO_ADC1_IN12);
|
||||||
if (adc_state != OK) {
|
//stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
|
||||||
/* Try again */
|
|
||||||
adc_state = adc_devinit();
|
|
||||||
|
|
||||||
if (adc_state != OK) {
|
|
||||||
/* Give up */
|
|
||||||
message("[boot] FAILED adc_devinit: %d\n", adc_state);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
|
@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void)
|
||||||
stm32_configgpio(GPIO_ACC_OC_DETECT);
|
stm32_configgpio(GPIO_ACC_OC_DETECT);
|
||||||
stm32_configgpio(GPIO_SERVO_OC_DETECT);
|
stm32_configgpio(GPIO_SERVO_OC_DETECT);
|
||||||
stm32_configgpio(GPIO_BTN_SAFETY);
|
stm32_configgpio(GPIO_BTN_SAFETY);
|
||||||
|
|
||||||
|
stm32_configgpio(GPIO_ADC_VBATT);
|
||||||
|
stm32_configgpio(GPIO_ADC_IN5);
|
||||||
}
|
}
|
||||||
|
|
|
@ -61,28 +61,6 @@
|
||||||
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
|
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
|
||||||
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
|
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
|
||||||
|
|
||||||
/* R/C in/out channels **************************************************************/
|
|
||||||
|
|
||||||
/* XXX just GPIOs for now - eventually timer pins */
|
|
||||||
|
|
||||||
#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
|
|
||||||
#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
|
|
||||||
#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
|
|
||||||
#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
|
|
||||||
#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
|
|
||||||
#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
|
|
||||||
#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
|
|
||||||
#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
|
|
||||||
|
|
||||||
#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
|
|
||||||
#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
|
|
||||||
#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
|
|
||||||
#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
|
|
||||||
#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
|
|
||||||
#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
|
|
||||||
#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
|
|
||||||
#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
|
|
||||||
|
|
||||||
/* Safety switch button *************************************************************/
|
/* Safety switch button *************************************************************/
|
||||||
|
|
||||||
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
|
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
|
||||||
|
@ -98,3 +76,8 @@
|
||||||
|
|
||||||
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
|
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
|
||||||
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
|
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
|
||||||
|
|
||||||
|
/* Analog inputs ********************************************************************/
|
||||||
|
|
||||||
|
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
|
||||||
|
#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
|
||||||
|
|
|
@ -0,0 +1,52 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 drv_adc.h
|
||||||
|
*
|
||||||
|
* ADC driver interface.
|
||||||
|
*
|
||||||
|
* This defines additional operations over and above the standard NuttX
|
||||||
|
* ADC API.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
|
#define ADC_DEVICE_PATH "/dev/adc0"
|
||||||
|
|
||||||
|
/*
|
||||||
|
* ioctl definitions
|
||||||
|
*/
|
|
@ -100,28 +100,13 @@ struct mixer_simple_s {
|
||||||
*/
|
*/
|
||||||
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
|
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
|
||||||
|
|
||||||
/** multirotor output definition */
|
/* _MIXERIOC(3) was deprecated */
|
||||||
struct mixer_rotor_output_s {
|
/* _MIXERIOC(4) was deprecated */
|
||||||
float angle; /**< rotor angle clockwise from forward in radians */
|
|
||||||
float distance; /**< motor distance from centre in arbitrary units */
|
|
||||||
};
|
|
||||||
|
|
||||||
/** multirotor mixer */
|
|
||||||
struct mixer_multirotor_s {
|
|
||||||
uint8_t rotor_count;
|
|
||||||
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
|
|
||||||
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg
|
* Add mixer(s) from the buffer in (const char *)arg
|
||||||
*/
|
*/
|
||||||
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
|
#define MIXERIOCLOADBUF _MIXERIOC(5)
|
||||||
|
|
||||||
/**
|
|
||||||
* Add mixers(s) from a the file in (const char *)arg
|
|
||||||
*/
|
|
||||||
#define MIXERIOCLOADFILE _MIXERIOC(4)
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* XXX Thoughts for additional operations:
|
* XXX Thoughts for additional operations:
|
||||||
|
|
|
@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm);
|
||||||
* Note that ioctls and ObjDev updates should not be mixed, as the
|
* Note that ioctls and ObjDev updates should not be mixed, as the
|
||||||
* behaviour of the system in this case is not defined.
|
* behaviour of the system in this case is not defined.
|
||||||
*/
|
*/
|
||||||
#define _PWM_SERVO_BASE 0x2700
|
#define _PWM_SERVO_BASE 0x2a00
|
||||||
|
|
||||||
/** arm all servo outputs handle by this driver */
|
/** arm all servo outputs handle by this driver */
|
||||||
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
|
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
|
||||||
|
@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
|
||||||
/** disarm all servo outputs (stop generating pulses) */
|
/** disarm all servo outputs (stop generating pulses) */
|
||||||
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
|
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
|
||||||
|
|
||||||
|
/** set update rate in Hz */
|
||||||
|
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
|
||||||
|
|
||||||
/** set a single servo to a specific value */
|
/** set a single servo to a specific value */
|
||||||
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
||||||
|
|
||||||
|
|
|
@ -68,11 +68,11 @@
|
||||||
#include <drivers/device/device.h>
|
#include <drivers/device/device.h>
|
||||||
#include <drivers/drv_pwm_output.h>
|
#include <drivers/drv_pwm_output.h>
|
||||||
#include <drivers/drv_gpio.h>
|
#include <drivers/drv_gpio.h>
|
||||||
// #include <drivers/boards/HIL/HIL_internal.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <drivers/drv_mixer.h>
|
||||||
|
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
#include <systemlib/mixer/mixer.h>
|
#include <systemlib/mixer/mixer.h>
|
||||||
#include <drivers/drv_mixer.h>
|
|
||||||
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
@ -382,7 +382,6 @@ HIL::task_main()
|
||||||
/* this would be bad... */
|
/* this would be bad... */
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
log("poll error %d", errno);
|
log("poll error %d", errno);
|
||||||
usleep(1000000);
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -396,16 +395,27 @@ HIL::task_main()
|
||||||
if (_mixers != nullptr) {
|
if (_mixers != nullptr) {
|
||||||
|
|
||||||
/* do mixing */
|
/* do mixing */
|
||||||
_mixers->mix(&outputs.output[0], num_outputs);
|
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
|
||||||
|
outputs.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
/* iterate actuators */
|
/* iterate actuators */
|
||||||
for (unsigned i = 0; i < num_outputs; i++) {
|
for (unsigned i = 0; i < num_outputs; i++) {
|
||||||
|
|
||||||
/* scale for PWM output 900 - 2100us */
|
/* last resort: catch NaN, INF and out-of-band errors */
|
||||||
outputs.output[i] = 1500 + (600 * outputs.output[i]);
|
if (i < (unsigned)outputs.noutputs &&
|
||||||
|
isfinite(outputs.output[i]) &&
|
||||||
/* output to the servo */
|
outputs.output[i] >= -1.0f &&
|
||||||
// up_pwm_servo_set(i, outputs.output[i]);
|
outputs.output[i] <= 1.0f) {
|
||||||
|
/* scale for PWM output 900 - 2100us */
|
||||||
|
outputs.output[i] = 1500 + (600 * outputs.output[i]);
|
||||||
|
} else {
|
||||||
|
/*
|
||||||
|
* Value is NaN, INF or out of band - set to the minimum value.
|
||||||
|
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||||
|
* spinning motors. It would be deadly in flight.
|
||||||
|
*/
|
||||||
|
outputs.output[i] = 900;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* and publish for anyone that cares to see */
|
/* and publish for anyone that cares to see */
|
||||||
|
@ -419,9 +429,6 @@ HIL::task_main()
|
||||||
|
|
||||||
/* get new value */
|
/* get new value */
|
||||||
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
|
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
|
||||||
|
|
||||||
/* update PWM servo armed status */
|
|
||||||
// up_pwm_servo_arm(aa.armed);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -503,6 +510,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
// up_pwm_servo_arm(false);
|
// up_pwm_servo_arm(false);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case PWM_SERVO_SET_UPDATE_RATE:
|
||||||
|
g_hil->set_pwm_rate(arg);
|
||||||
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_SET(2):
|
case PWM_SERVO_SET(2):
|
||||||
case PWM_SERVO_SET(3):
|
case PWM_SERVO_SET(3):
|
||||||
if (_mode != MODE_4PWM) {
|
if (_mode != MODE_4PWM) {
|
||||||
|
@ -577,26 +588,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MIXERIOCADDMULTIROTOR:
|
case MIXERIOCLOADBUF: {
|
||||||
/* XXX not yet supported */
|
const char *buf = (const char *)arg;
|
||||||
ret = -ENOTTY;
|
unsigned buflen = strnlen(buf, 1024);
|
||||||
break;
|
|
||||||
|
|
||||||
case MIXERIOCLOADFILE: {
|
if (_mixers == nullptr)
|
||||||
const char *path = (const char *)arg;
|
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
||||||
|
|
||||||
if (_mixers != nullptr) {
|
|
||||||
delete _mixers;
|
|
||||||
_mixers = nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
|
||||||
if (_mixers == nullptr) {
|
if (_mixers == nullptr) {
|
||||||
ret = -ENOMEM;
|
ret = -ENOMEM;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
debug("loading mixers from %s", path);
|
ret = _mixers->load_from_buf(buf, buflen);
|
||||||
ret = _mixers->load_from_file(path);
|
|
||||||
|
|
||||||
if (ret != 0) {
|
if (ret != 0) {
|
||||||
debug("mixer load failed with %d", ret);
|
debug("mixer load failed with %d", ret);
|
||||||
|
@ -605,10 +609,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
ret = -EINVAL;
|
ret = -EINVAL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
default:
|
default:
|
||||||
ret = -ENOTTY;
|
ret = -ENOTTY;
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -1080,10 +1080,10 @@ int HMC5883::check_offset()
|
||||||
|
|
||||||
int HMC5883::check_calibration()
|
int HMC5883::check_calibration()
|
||||||
{
|
{
|
||||||
bool offset_valid = !(check_offset() == OK);
|
bool offset_valid = (check_offset() == OK);
|
||||||
bool scale_valid = !(check_scale() == 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 ",
|
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
|
||||||
(offset_valid) ? "" : "offset invalid");
|
(offset_valid) ? "" : "offset invalid");
|
||||||
_calibrated = (offset_valid && scale_valid);
|
_calibrated = (offset_valid && scale_valid);
|
||||||
|
|
|
@ -446,8 +446,12 @@ MPU6000::init()
|
||||||
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
|
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
|
||||||
usleep(1000);
|
usleep(1000);
|
||||||
|
|
||||||
/* do CDev init for the gyro device node */
|
/* do CDev init for the gyro device node, keep it optional */
|
||||||
ret = _gyro->init();
|
int gyro_ret = _gyro->init();
|
||||||
|
|
||||||
|
if (gyro_ret != OK) {
|
||||||
|
_gyro_topic = -1;
|
||||||
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
@ -938,7 +942,9 @@ MPU6000::measure()
|
||||||
|
|
||||||
/* and publish for subscribers */
|
/* and publish for subscribers */
|
||||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
|
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
|
||||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
|
if (_gyro_topic != -1) {
|
||||||
|
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
|
||||||
|
}
|
||||||
|
|
||||||
/* stop measuring */
|
/* stop measuring */
|
||||||
perf_end(_sample_perf);
|
perf_end(_sample_perf);
|
||||||
|
|
|
@ -58,12 +58,15 @@
|
||||||
#include <drivers/drv_pwm_output.h>
|
#include <drivers/drv_pwm_output.h>
|
||||||
#include <drivers/drv_gpio.h>
|
#include <drivers/drv_gpio.h>
|
||||||
#include <drivers/boards/px4fmu/px4fmu_internal.h>
|
#include <drivers/boards/px4fmu/px4fmu_internal.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <systemlib/err.h>
|
||||||
#include <systemlib/mixer/mixer.h>
|
#include <systemlib/mixer/mixer.h>
|
||||||
#include <drivers/drv_mixer.h>
|
#include <drivers/drv_mixer.h>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
@ -96,6 +99,7 @@ private:
|
||||||
int _t_actuators;
|
int _t_actuators;
|
||||||
int _t_armed;
|
int _t_armed;
|
||||||
orb_advert_t _t_outputs;
|
orb_advert_t _t_outputs;
|
||||||
|
orb_advert_t _t_actuators_effective;
|
||||||
unsigned _num_outputs;
|
unsigned _num_outputs;
|
||||||
bool _primary_pwm_device;
|
bool _primary_pwm_device;
|
||||||
|
|
||||||
|
@ -110,9 +114,9 @@ private:
|
||||||
void task_main() __attribute__((noreturn));
|
void task_main() __attribute__((noreturn));
|
||||||
|
|
||||||
static int control_callback(uintptr_t handle,
|
static int control_callback(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input);
|
float &input);
|
||||||
|
|
||||||
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
|
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
|
@ -161,6 +165,7 @@ PX4FMU::PX4FMU() :
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
_t_armed(-1),
|
_t_armed(-1),
|
||||||
_t_outputs(0),
|
_t_outputs(0),
|
||||||
|
_t_actuators_effective(0),
|
||||||
_num_outputs(0),
|
_num_outputs(0),
|
||||||
_primary_pwm_device(false),
|
_primary_pwm_device(false),
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
|
@ -177,6 +182,7 @@ PX4FMU::~PX4FMU()
|
||||||
_task_should_exit = true;
|
_task_should_exit = true;
|
||||||
|
|
||||||
unsigned i = 10;
|
unsigned i = 10;
|
||||||
|
|
||||||
do {
|
do {
|
||||||
/* wait 50ms - it should wake every 100ms or so worst-case */
|
/* wait 50ms - it should wake every 100ms or so worst-case */
|
||||||
usleep(50000);
|
usleep(50000);
|
||||||
|
@ -212,6 +218,7 @@ PX4FMU::init()
|
||||||
|
|
||||||
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
|
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
|
||||||
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
|
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
log("default PWM output device");
|
log("default PWM output device");
|
||||||
_primary_pwm_device = true;
|
_primary_pwm_device = true;
|
||||||
|
@ -245,7 +252,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[])
|
||||||
int
|
int
|
||||||
PX4FMU::set_mode(Mode mode)
|
PX4FMU::set_mode(Mode mode)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
* Configure for PWM output.
|
* Configure for PWM output.
|
||||||
*
|
*
|
||||||
* Note that regardless of the configured mode, the task is always
|
* Note that regardless of the configured mode, the task is always
|
||||||
|
@ -279,6 +286,7 @@ PX4FMU::set_mode(Mode mode)
|
||||||
default:
|
default:
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
_mode = mode;
|
_mode = mode;
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
@ -315,6 +323,13 @@ PX4FMU::task_main()
|
||||||
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
|
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
|
||||||
&outputs);
|
&outputs);
|
||||||
|
|
||||||
|
/* advertise the effective control inputs */
|
||||||
|
actuator_controls_effective_s controls_effective;
|
||||||
|
memset(&controls_effective, 0, sizeof(controls_effective));
|
||||||
|
/* advertise the effective control inputs */
|
||||||
|
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
|
||||||
|
&controls_effective);
|
||||||
|
|
||||||
pollfd fds[2];
|
pollfd fds[2];
|
||||||
fds[0].fd = _t_actuators;
|
fds[0].fd = _t_actuators;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
|
@ -331,8 +346,18 @@ PX4FMU::task_main()
|
||||||
/* handle update rate changes */
|
/* handle update rate changes */
|
||||||
if (_current_update_rate != _update_rate) {
|
if (_current_update_rate != _update_rate) {
|
||||||
int update_rate_in_ms = int(1000 / _update_rate);
|
int update_rate_in_ms = int(1000 / _update_rate);
|
||||||
if (update_rate_in_ms < 2)
|
|
||||||
|
/* reject faster than 500 Hz updates */
|
||||||
|
if (update_rate_in_ms < 2) {
|
||||||
update_rate_in_ms = 2;
|
update_rate_in_ms = 2;
|
||||||
|
_update_rate = 500;
|
||||||
|
}
|
||||||
|
/* reject slower than 50 Hz updates */
|
||||||
|
if (update_rate_in_ms > 20) {
|
||||||
|
update_rate_in_ms = 20;
|
||||||
|
_update_rate = 50;
|
||||||
|
}
|
||||||
|
|
||||||
orb_set_interval(_t_actuators, update_rate_in_ms);
|
orb_set_interval(_t_actuators, update_rate_in_ms);
|
||||||
up_pwm_servo_set_rate(_update_rate);
|
up_pwm_servo_set_rate(_update_rate);
|
||||||
_current_update_rate = _update_rate;
|
_current_update_rate = _update_rate;
|
||||||
|
@ -358,20 +383,39 @@ PX4FMU::task_main()
|
||||||
if (_mixers != nullptr) {
|
if (_mixers != nullptr) {
|
||||||
|
|
||||||
/* do mixing */
|
/* do mixing */
|
||||||
_mixers->mix(&outputs.output[0], num_outputs);
|
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
|
||||||
|
outputs.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
// XXX output actual limited values
|
||||||
|
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
|
||||||
|
|
||||||
|
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
|
||||||
|
|
||||||
/* iterate actuators */
|
/* iterate actuators */
|
||||||
for (unsigned i = 0; i < num_outputs; i++) {
|
for (unsigned i = 0; i < num_outputs; i++) {
|
||||||
|
|
||||||
/* scale for PWM output 900 - 2100us */
|
/* last resort: catch NaN, INF and out-of-band errors */
|
||||||
outputs.output[i] = 1500 + (600 * outputs.output[i]);
|
if (i < outputs.noutputs &&
|
||||||
|
isfinite(outputs.output[i]) &&
|
||||||
|
outputs.output[i] >= -1.0f &&
|
||||||
|
outputs.output[i] <= 1.0f) {
|
||||||
|
/* scale for PWM output 900 - 2100us */
|
||||||
|
outputs.output[i] = 1500 + (600 * outputs.output[i]);
|
||||||
|
} else {
|
||||||
|
/*
|
||||||
|
* Value is NaN, INF or out of band - set to the minimum value.
|
||||||
|
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||||
|
* spinning motors. It would be deadly in flight.
|
||||||
|
*/
|
||||||
|
outputs.output[i] = 900;
|
||||||
|
}
|
||||||
|
|
||||||
/* output to the servo */
|
/* output to the servo */
|
||||||
up_pwm_servo_set(i, outputs.output[i]);
|
up_pwm_servo_set(i, outputs.output[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* and publish for anyone that cares to see */
|
/* and publish for anyone that cares to see */
|
||||||
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
|
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -388,6 +432,7 @@ PX4FMU::task_main()
|
||||||
}
|
}
|
||||||
|
|
||||||
::close(_t_actuators);
|
::close(_t_actuators);
|
||||||
|
::close(_t_actuators_effective);
|
||||||
::close(_t_armed);
|
::close(_t_armed);
|
||||||
|
|
||||||
/* make sure servos are off */
|
/* make sure servos are off */
|
||||||
|
@ -404,9 +449,9 @@ PX4FMU::task_main()
|
||||||
|
|
||||||
int
|
int
|
||||||
PX4FMU::control_callback(uintptr_t handle,
|
PX4FMU::control_callback(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input)
|
float &input)
|
||||||
{
|
{
|
||||||
const actuator_controls_s *controls = (actuator_controls_s *)handle;
|
const actuator_controls_s *controls = (actuator_controls_s *)handle;
|
||||||
|
|
||||||
|
@ -424,15 +469,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
/* try it as a GPIO ioctl first */
|
/* try it as a GPIO ioctl first */
|
||||||
ret = gpio_ioctl(filp, cmd, arg);
|
ret = gpio_ioctl(filp, cmd, arg);
|
||||||
|
|
||||||
if (ret != -ENOTTY)
|
if (ret != -ENOTTY)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
|
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
|
||||||
switch(_mode) {
|
switch (_mode) {
|
||||||
case MODE_2PWM:
|
case MODE_2PWM:
|
||||||
case MODE_4PWM:
|
case MODE_4PWM:
|
||||||
ret = pwm_ioctl(filp, cmd, arg);
|
ret = pwm_ioctl(filp, cmd, arg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
debug("not in a PWM mode");
|
debug("not in a PWM mode");
|
||||||
break;
|
break;
|
||||||
|
@ -462,6 +509,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
up_pwm_servo_arm(false);
|
up_pwm_servo_arm(false);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case PWM_SERVO_SET_UPDATE_RATE:
|
||||||
|
set_pwm_rate(arg);
|
||||||
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_SET(2):
|
case PWM_SERVO_SET(2):
|
||||||
case PWM_SERVO_SET(3):
|
case PWM_SERVO_SET(3):
|
||||||
if (_mode != MODE_4PWM) {
|
if (_mode != MODE_4PWM) {
|
||||||
|
@ -492,7 +543,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
/* FALLTHROUGH */
|
/* FALLTHROUGH */
|
||||||
case PWM_SERVO_GET(0):
|
case PWM_SERVO_GET(0):
|
||||||
case PWM_SERVO_GET(1): {
|
case PWM_SERVO_GET(1): {
|
||||||
channel = cmd - PWM_SERVO_SET(0);
|
channel = cmd - PWM_SERVO_GET(0);
|
||||||
*(servo_position_t *)arg = up_pwm_servo_get(channel);
|
*(servo_position_t *)arg = up_pwm_servo_get(channel);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -536,26 +587,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MIXERIOCADDMULTIROTOR:
|
case MIXERIOCLOADBUF: {
|
||||||
/* XXX not yet supported */
|
const char *buf = (const char *)arg;
|
||||||
ret = -ENOTTY;
|
unsigned buflen = strnlen(buf, 1024);
|
||||||
break;
|
|
||||||
|
|
||||||
case MIXERIOCLOADFILE: {
|
if (_mixers == nullptr)
|
||||||
const char *path = (const char *)arg;
|
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
||||||
|
|
||||||
if (_mixers != nullptr) {
|
|
||||||
delete _mixers;
|
|
||||||
_mixers = nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
|
||||||
if (_mixers == nullptr) {
|
if (_mixers == nullptr) {
|
||||||
ret = -ENOMEM;
|
ret = -ENOMEM;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
debug("loading mixers from %s", path);
|
ret = _mixers->load_from_buf(buf, buflen);
|
||||||
ret = _mixers->load_from_file(path);
|
|
||||||
|
|
||||||
if (ret != 0) {
|
if (ret != 0) {
|
||||||
debug("mixer load failed with %d", ret);
|
debug("mixer load failed with %d", ret);
|
||||||
|
@ -564,7 +608,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
ret = -EINVAL;
|
ret = -EINVAL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -582,7 +625,7 @@ void
|
||||||
PX4FMU::gpio_reset(void)
|
PX4FMU::gpio_reset(void)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
|
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
|
||||||
* to input mode.
|
* to input mode.
|
||||||
*/
|
*/
|
||||||
for (unsigned i = 0; i < _ngpio; i++)
|
for (unsigned i = 0; i < _ngpio; i++)
|
||||||
|
@ -609,17 +652,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
|
||||||
|
|
||||||
/* configure selected GPIOs as required */
|
/* configure selected GPIOs as required */
|
||||||
for (unsigned i = 0; i < _ngpio; i++) {
|
for (unsigned i = 0; i < _ngpio; i++) {
|
||||||
if (gpios & (1<<i)) {
|
if (gpios & (1 << i)) {
|
||||||
switch (function) {
|
switch (function) {
|
||||||
case GPIO_SET_INPUT:
|
case GPIO_SET_INPUT:
|
||||||
stm32_configgpio(_gpio_tab[i].input);
|
stm32_configgpio(_gpio_tab[i].input);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case GPIO_SET_OUTPUT:
|
case GPIO_SET_OUTPUT:
|
||||||
stm32_configgpio(_gpio_tab[i].output);
|
stm32_configgpio(_gpio_tab[i].output);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case GPIO_SET_ALT_1:
|
case GPIO_SET_ALT_1:
|
||||||
if (_gpio_tab[i].alt != 0)
|
if (_gpio_tab[i].alt != 0)
|
||||||
stm32_configgpio(_gpio_tab[i].alt);
|
stm32_configgpio(_gpio_tab[i].alt);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -636,7 +682,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function)
|
||||||
int value = (function == GPIO_SET) ? 1 : 0;
|
int value = (function == GPIO_SET) ? 1 : 0;
|
||||||
|
|
||||||
for (unsigned i = 0; i < _ngpio; i++)
|
for (unsigned i = 0; i < _ngpio; i++)
|
||||||
if (gpios & (1<<i))
|
if (gpios & (1 << i))
|
||||||
stm32_gpiowrite(_gpio_tab[i].output, value);
|
stm32_gpiowrite(_gpio_tab[i].output, value);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -660,7 +706,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
lock();
|
lock();
|
||||||
|
|
||||||
switch (cmd) {
|
switch (cmd) {
|
||||||
|
|
||||||
case GPIO_RESET:
|
case GPIO_RESET:
|
||||||
gpio_reset();
|
gpio_reset();
|
||||||
break;
|
break;
|
||||||
|
@ -762,6 +808,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
|
||||||
|
|
||||||
/* (re)set the PWM output mode */
|
/* (re)set the PWM output mode */
|
||||||
g_fmu->set_mode(servo_mode);
|
g_fmu->set_mode(servo_mode);
|
||||||
|
|
||||||
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
|
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
|
||||||
g_fmu->set_pwm_rate(update_rate);
|
g_fmu->set_pwm_rate(update_rate);
|
||||||
|
|
||||||
|
@ -800,13 +847,18 @@ test(void)
|
||||||
|
|
||||||
fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
|
fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
|
||||||
|
|
||||||
if (fd < 0) {
|
if (fd < 0)
|
||||||
puts("open fail");
|
errx(1, "open fail");
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
ioctl(fd, PWM_SERVO_ARM, 0);
|
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
|
||||||
ioctl(fd, PWM_SERVO_SET(0), 1000);
|
|
||||||
|
if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
|
||||||
|
|
||||||
|
if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
|
||||||
|
|
||||||
|
if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
|
||||||
|
|
||||||
|
if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
|
@ -816,10 +868,8 @@ test(void)
|
||||||
void
|
void
|
||||||
fake(int argc, char *argv[])
|
fake(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
if (argc < 5) {
|
if (argc < 5)
|
||||||
puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
|
errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
actuator_controls_s ac;
|
actuator_controls_s ac;
|
||||||
|
|
||||||
|
@ -833,10 +883,18 @@ fake(int argc, char *argv[])
|
||||||
|
|
||||||
orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
|
orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
|
||||||
|
|
||||||
if (handle < 0) {
|
if (handle < 0)
|
||||||
puts("advertise failed");
|
errx(1, "advertise failed");
|
||||||
exit(1);
|
|
||||||
}
|
actuator_armed_s aa;
|
||||||
|
|
||||||
|
aa.armed = true;
|
||||||
|
aa.lockdown = false;
|
||||||
|
|
||||||
|
handle = orb_advertise(ORB_ID(actuator_armed), &aa);
|
||||||
|
|
||||||
|
if (handle < 0)
|
||||||
|
errx(1, "advertise failed 2");
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
@ -890,15 +948,17 @@ fmu_main(int argc, char *argv[])
|
||||||
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
|
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
|
||||||
if (argc > i + 1) {
|
if (argc > i + 1) {
|
||||||
pwm_update_rate_in_hz = atoi(argv[i + 1]);
|
pwm_update_rate_in_hz = atoi(argv[i + 1]);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
|
errx(1, "missing argument for pwm update rate (-u)");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
|
errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* was a new mode set? */
|
/* was a new mode set? */
|
||||||
if (new_mode != PORT_MODE_UNSET) {
|
if (new_mode != PORT_MODE_UNSET) {
|
||||||
|
@ -915,5 +975,5 @@ fmu_main(int argc, char *argv[])
|
||||||
|
|
||||||
fprintf(stderr, "FMU: unrecognised command, try:\n");
|
fprintf(stderr, "FMU: unrecognised command, try:\n");
|
||||||
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
|
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
|
||||||
return -EINVAL;
|
exit(1);
|
||||||
}
|
}
|
||||||
|
|
|
@ -55,12 +55,14 @@
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <termios.h>
|
#include <termios.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
#include <arch/board/board.h>
|
#include <arch/board/board.h>
|
||||||
|
|
||||||
#include <drivers/device/device.h>
|
#include <drivers/device/device.h>
|
||||||
#include <drivers/drv_rc_input.h>
|
#include <drivers/drv_rc_input.h>
|
||||||
#include <drivers/drv_pwm_output.h>
|
#include <drivers/drv_pwm_output.h>
|
||||||
|
#include <drivers/drv_gpio.h>
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
#include <drivers/drv_mixer.h>
|
#include <drivers/drv_mixer.h>
|
||||||
|
|
||||||
|
@ -69,10 +71,15 @@
|
||||||
#include <systemlib/hx_stream.h>
|
#include <systemlib/hx_stream.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <systemlib/scheduling_priorities.h>
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/rc_channels.h>
|
#include <uORB/topics/rc_channels.h>
|
||||||
|
#include <uORB/topics/battery_status.h>
|
||||||
|
|
||||||
#include <px4io/protocol.h>
|
#include <px4io/protocol.h>
|
||||||
#include "uploader.h"
|
#include "uploader.h"
|
||||||
|
@ -88,8 +95,18 @@ public:
|
||||||
|
|
||||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the PWM via serial update rate
|
||||||
|
* @warning this directly affects CPU load
|
||||||
|
*/
|
||||||
|
int set_pwm_rate(int hz);
|
||||||
|
|
||||||
|
bool dump_one;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
|
// XXX
|
||||||
|
static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
|
||||||
|
unsigned _update_rate; ///< serial send rate in Hz
|
||||||
|
|
||||||
int _serial_fd; ///< serial interface to PX4IO
|
int _serial_fd; ///< serial interface to PX4IO
|
||||||
hx_stream_t _io_stream; ///< HX protocol stream
|
hx_stream_t _io_stream; ///< HX protocol stream
|
||||||
|
@ -101,19 +118,30 @@ private:
|
||||||
int _t_actuators; ///< actuator output topic
|
int _t_actuators; ///< actuator output topic
|
||||||
actuator_controls_s _controls; ///< actuator outputs
|
actuator_controls_s _controls; ///< actuator outputs
|
||||||
|
|
||||||
|
orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
|
||||||
|
actuator_controls_effective_s _controls_effective; ///< effective controls
|
||||||
|
|
||||||
int _t_armed; ///< system armed control topic
|
int _t_armed; ///< system armed control topic
|
||||||
actuator_armed_s _armed; ///< system armed state
|
actuator_armed_s _armed; ///< system armed state
|
||||||
|
int _t_vstatus; ///< system / vehicle status
|
||||||
|
vehicle_status_s _vstatus; ///< overall system state
|
||||||
|
|
||||||
orb_advert_t _to_input_rc; ///< rc inputs from io
|
orb_advert_t _to_input_rc; ///< rc inputs from io
|
||||||
rc_input_values _input_rc; ///< rc input values
|
rc_input_values _input_rc; ///< rc input values
|
||||||
|
|
||||||
|
orb_advert_t _to_battery; ///< battery status / voltage
|
||||||
|
battery_status_s _battery_status;///< battery status data
|
||||||
|
|
||||||
orb_advert_t _t_outputs; ///< mixed outputs topic
|
orb_advert_t _t_outputs; ///< mixed outputs topic
|
||||||
actuator_outputs_s _outputs; ///< mixed outputs
|
actuator_outputs_s _outputs; ///< mixed outputs
|
||||||
|
|
||||||
MixerGroup *_mixers; ///< loaded mixers
|
const char *volatile _mix_buf; ///< mixer text buffer
|
||||||
|
volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
|
||||||
|
|
||||||
bool _primary_pwm_device; ///< true if we are the default PWM output
|
bool _primary_pwm_device; ///< true if we are the default PWM output
|
||||||
|
|
||||||
|
uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
|
||||||
|
|
||||||
volatile bool _switch_armed; ///< PX4IO switch armed state
|
volatile bool _switch_armed; ///< PX4IO switch armed state
|
||||||
// XXX how should this work?
|
// XXX how should this work?
|
||||||
|
|
||||||
|
@ -155,6 +183,11 @@ private:
|
||||||
*/
|
*/
|
||||||
void config_send();
|
void config_send();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Send a buffer containing mixer text to PX4IO
|
||||||
|
*/
|
||||||
|
int mixer_send(const char *buf, unsigned buflen);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Mixer control callback; invoked to fetch a control from a specific
|
* Mixer control callback; invoked to fetch a control from a specific
|
||||||
* group/index during mixing.
|
* group/index during mixing.
|
||||||
|
@ -175,19 +208,25 @@ PX4IO *g_dev;
|
||||||
|
|
||||||
PX4IO::PX4IO() :
|
PX4IO::PX4IO() :
|
||||||
CDev("px4io", "/dev/px4io"),
|
CDev("px4io", "/dev/px4io"),
|
||||||
|
dump_one(false),
|
||||||
|
_update_rate(50),
|
||||||
_serial_fd(-1),
|
_serial_fd(-1),
|
||||||
_io_stream(nullptr),
|
_io_stream(nullptr),
|
||||||
_task(-1),
|
_task(-1),
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
_connected(false),
|
_connected(false),
|
||||||
_t_actuators(-1),
|
_t_actuators(-1),
|
||||||
|
_t_actuators_effective(-1),
|
||||||
_t_armed(-1),
|
_t_armed(-1),
|
||||||
|
_t_vstatus(-1),
|
||||||
_t_outputs(-1),
|
_t_outputs(-1),
|
||||||
_mixers(nullptr),
|
_mix_buf(nullptr),
|
||||||
|
_mix_buf_len(0),
|
||||||
_primary_pwm_device(false),
|
_primary_pwm_device(false),
|
||||||
|
_relays(0),
|
||||||
_switch_armed(false),
|
_switch_armed(false),
|
||||||
_send_needed(false),
|
_send_needed(false),
|
||||||
_config_needed(false)
|
_config_needed(true)
|
||||||
{
|
{
|
||||||
/* we need this potentially before it could be set in task_main */
|
/* we need this potentially before it could be set in task_main */
|
||||||
g_dev = this;
|
g_dev = this;
|
||||||
|
@ -205,6 +244,7 @@ PX4IO::~PX4IO()
|
||||||
/* give it another 100ms */
|
/* give it another 100ms */
|
||||||
usleep(100000);
|
usleep(100000);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* well, kill it anyway, though this will probably crash */
|
/* well, kill it anyway, though this will probably crash */
|
||||||
if (_task != -1)
|
if (_task != -1)
|
||||||
task_delete(_task);
|
task_delete(_task);
|
||||||
|
@ -234,7 +274,8 @@ PX4IO::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* start the IO interface task */
|
/* start the IO interface task */
|
||||||
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
|
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
|
||||||
|
|
||||||
if (_task < 0) {
|
if (_task < 0) {
|
||||||
debug("task start failed: %d", errno);
|
debug("task start failed: %d", errno);
|
||||||
return -errno;
|
return -errno;
|
||||||
|
@ -246,16 +287,30 @@ PX4IO::init()
|
||||||
debug("PX4IO connected");
|
debug("PX4IO connected");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
usleep(100000);
|
usleep(100000);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_connected) {
|
if (!_connected) {
|
||||||
/* error here will result in everything being torn down */
|
/* error here will result in everything being torn down */
|
||||||
log("PX4IO not responding");
|
log("PX4IO not responding");
|
||||||
return -EIO;
|
return -EIO;
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
PX4IO::set_pwm_rate(int hz)
|
||||||
|
{
|
||||||
|
if (hz > 0 && hz <= 400) {
|
||||||
|
_update_rate = hz;
|
||||||
|
return OK;
|
||||||
|
} else {
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
PX4IO::task_main_trampoline(int argc, char *argv[])
|
PX4IO::task_main_trampoline(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
|
@ -266,6 +321,7 @@ void
|
||||||
PX4IO::task_main()
|
PX4IO::task_main()
|
||||||
{
|
{
|
||||||
log("starting");
|
log("starting");
|
||||||
|
unsigned update_rate_in_ms;
|
||||||
|
|
||||||
/* open the serial port */
|
/* open the serial port */
|
||||||
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
|
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
|
||||||
|
@ -287,10 +343,12 @@ PX4IO::task_main()
|
||||||
|
|
||||||
/* protocol stream */
|
/* protocol stream */
|
||||||
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
|
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
|
||||||
|
|
||||||
if (_io_stream == nullptr) {
|
if (_io_stream == nullptr) {
|
||||||
log("failed to allocate HX protocol stream");
|
log("failed to allocate HX protocol stream");
|
||||||
goto out;
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
hx_stream_set_counters(_io_stream,
|
hx_stream_set_counters(_io_stream,
|
||||||
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
|
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
|
||||||
perf_alloc(PC_COUNT, "PX4IO frames received"),
|
perf_alloc(PC_COUNT, "PX4IO frames received"),
|
||||||
|
@ -303,12 +361,20 @@ PX4IO::task_main()
|
||||||
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||||
ORB_ID(actuator_controls_1));
|
ORB_ID(actuator_controls_1));
|
||||||
/* convert the update rate in hz to milliseconds, rounding down if necessary */
|
/* convert the update rate in hz to milliseconds, rounding down if necessary */
|
||||||
//int update_rate_in_ms = int(1000 / _update_rate);
|
update_rate_in_ms = 1000 / _update_rate;
|
||||||
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
|
orb_set_interval(_t_actuators, update_rate_in_ms);
|
||||||
|
|
||||||
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
|
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
|
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
|
||||||
|
|
||||||
|
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
|
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
|
||||||
|
|
||||||
|
/* advertise the limited control inputs */
|
||||||
|
memset(&_controls_effective, 0, sizeof(_controls_effective));
|
||||||
|
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
|
||||||
|
&_controls_effective);
|
||||||
|
|
||||||
/* advertise the mixed control outputs */
|
/* advertise the mixed control outputs */
|
||||||
memset(&_outputs, 0, sizeof(_outputs));
|
memset(&_outputs, 0, sizeof(_outputs));
|
||||||
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
|
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
|
||||||
|
@ -318,22 +384,33 @@ PX4IO::task_main()
|
||||||
memset(&_input_rc, 0, sizeof(_input_rc));
|
memset(&_input_rc, 0, sizeof(_input_rc));
|
||||||
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
|
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
|
||||||
|
|
||||||
|
/* do not advertise the battery status until its clear that a battery is connected */
|
||||||
|
memset(&_battery_status, 0, sizeof(_battery_status));
|
||||||
|
_to_battery = -1;
|
||||||
|
|
||||||
/* poll descriptor */
|
/* poll descriptor */
|
||||||
pollfd fds[3];
|
pollfd fds[4];
|
||||||
fds[0].fd = _serial_fd;
|
fds[0].fd = _serial_fd;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
fds[1].fd = _t_actuators;
|
fds[1].fd = _t_actuators;
|
||||||
fds[1].events = POLLIN;
|
fds[1].events = POLLIN;
|
||||||
fds[2].fd = _t_armed;
|
fds[2].fd = _t_armed;
|
||||||
fds[2].events = POLLIN;
|
fds[2].events = POLLIN;
|
||||||
|
fds[3].fd = _t_vstatus;
|
||||||
|
fds[3].events = POLLIN;
|
||||||
|
|
||||||
log("ready");
|
debug("ready");
|
||||||
|
|
||||||
|
/* lock against the ioctl handler */
|
||||||
|
lock();
|
||||||
|
|
||||||
/* loop handling received serial bytes */
|
/* loop handling received serial bytes */
|
||||||
while (!_task_should_exit) {
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
/* sleep waiting for data, but no more than 100ms */
|
/* sleep waiting for data, but no more than 100ms */
|
||||||
|
unlock();
|
||||||
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
|
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
|
||||||
|
lock();
|
||||||
|
|
||||||
/* this would be bad... */
|
/* this would be bad... */
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
|
@ -350,37 +427,32 @@ PX4IO::task_main()
|
||||||
if (fds[0].revents & POLLIN)
|
if (fds[0].revents & POLLIN)
|
||||||
io_recv();
|
io_recv();
|
||||||
|
|
||||||
/* if we have new data from the ORB, go handle it */
|
/* if we have new control data from the ORB, handle it */
|
||||||
if (fds[1].revents & POLLIN) {
|
if (fds[1].revents & POLLIN) {
|
||||||
|
|
||||||
/* get controls */
|
/* get controls */
|
||||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
|
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||||
|
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
|
||||||
|
|
||||||
/* mix */
|
/* scale controls to PWM (temporary measure) */
|
||||||
if (_mixers != nullptr) {
|
for (unsigned i = 0; i < _max_actuators; i++)
|
||||||
/* XXX is this the right count? */
|
_outputs.output[i] = 1500 + (600 * _controls.control[i]);
|
||||||
_mixers->mix(&_outputs.output[0], _max_actuators);
|
|
||||||
|
|
||||||
/* convert to PWM values */
|
/* and flag for update */
|
||||||
for (unsigned i = 0; i < _max_actuators; i++)
|
_send_needed = true;
|
||||||
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
|
|
||||||
|
|
||||||
/* and flag for update */
|
|
||||||
_send_needed = true;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* if we have an arming state update, handle it */
|
||||||
if (fds[2].revents & POLLIN) {
|
if (fds[2].revents & POLLIN) {
|
||||||
|
|
||||||
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
|
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
|
||||||
_send_needed = true;
|
_send_needed = true;
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
/* send an update to IO if required */
|
if (fds[3].revents & POLLIN) {
|
||||||
if (_send_needed) {
|
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
|
||||||
_send_needed = false;
|
_send_needed = true;
|
||||||
io_send();
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* send a config packet to IO if required */
|
/* send a config packet to IO if required */
|
||||||
|
@ -388,14 +460,32 @@ PX4IO::task_main()
|
||||||
_config_needed = false;
|
_config_needed = false;
|
||||||
config_send();
|
config_send();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* send a mixer update if needed */
|
||||||
|
if (_mix_buf != nullptr) {
|
||||||
|
mixer_send(_mix_buf, _mix_buf_len);
|
||||||
|
|
||||||
|
/* clear the buffer record so the ioctl handler knows we're done */
|
||||||
|
_mix_buf = nullptr;
|
||||||
|
_mix_buf_len = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* send an update to IO if required */
|
||||||
|
if (_send_needed) {
|
||||||
|
_send_needed = false;
|
||||||
|
io_send();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
unlock();
|
||||||
|
|
||||||
out:
|
out:
|
||||||
debug("exiting");
|
debug("exiting");
|
||||||
|
|
||||||
/* kill the HX stream */
|
/* kill the HX stream */
|
||||||
if (_io_stream != nullptr)
|
if (_io_stream != nullptr)
|
||||||
hx_stream_free(_io_stream);
|
hx_stream_free(_io_stream);
|
||||||
|
|
||||||
::close(_serial_fd);
|
::close(_serial_fd);
|
||||||
|
|
||||||
/* clean up the alternate device node */
|
/* clean up the alternate device node */
|
||||||
|
@ -448,25 +538,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
|
||||||
{
|
{
|
||||||
const px4io_report *rep = (const px4io_report *)buffer;
|
const px4io_report *rep = (const px4io_report *)buffer;
|
||||||
|
|
||||||
lock();
|
// lock();
|
||||||
|
|
||||||
/* sanity-check the received frame size */
|
/* sanity-check the received frame size */
|
||||||
if (bytes_received != sizeof(px4io_report)) {
|
if (bytes_received != sizeof(px4io_report)) {
|
||||||
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
|
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
|
||||||
goto out;
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (rep->i2f_magic != I2F_MAGIC) {
|
if (rep->i2f_magic != I2F_MAGIC) {
|
||||||
debug("bad magic");
|
debug("bad magic");
|
||||||
goto out;
|
goto out;
|
||||||
}
|
}
|
||||||
|
|
||||||
_connected = true;
|
_connected = true;
|
||||||
|
|
||||||
/* publish raw rc channel values from IO if valid channels are present */
|
/* publish raw rc channel values from IO if valid channels are present */
|
||||||
if (rep->channel_count > 0) {
|
if (rep->channel_count > 0) {
|
||||||
_input_rc.timestamp = hrt_absolute_time();
|
_input_rc.timestamp = hrt_absolute_time();
|
||||||
_input_rc.channel_count = rep->channel_count;
|
_input_rc.channel_count = rep->channel_count;
|
||||||
for (int i = 0; i < rep->channel_count; i++)
|
|
||||||
{
|
for (int i = 0; i < rep->channel_count; i++) {
|
||||||
_input_rc.values[i] = rep->rc_channel[i];
|
_input_rc.values[i] = rep->rc_channel[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -476,10 +568,40 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
|
||||||
/* remember the latched arming switch state */
|
/* remember the latched arming switch state */
|
||||||
_switch_armed = rep->armed;
|
_switch_armed = rep->armed;
|
||||||
|
|
||||||
|
/* publish battery information */
|
||||||
|
|
||||||
|
/* only publish if battery has a valid minimum voltage */
|
||||||
|
if (rep->battery_mv > 3300) {
|
||||||
|
_battery_status.timestamp = hrt_absolute_time();
|
||||||
|
_battery_status.voltage_v = rep->battery_mv / 1000.0f;
|
||||||
|
/* current and discharge are unknown */
|
||||||
|
_battery_status.current_a = -1.0f;
|
||||||
|
_battery_status.discharged_mah = -1.0f;
|
||||||
|
/* announce the battery voltage if needed, just publish else */
|
||||||
|
if (_to_battery > 0) {
|
||||||
|
orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
|
||||||
|
} else {
|
||||||
|
_to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
_send_needed = true;
|
_send_needed = true;
|
||||||
|
|
||||||
|
/* if monitoring, dump the received info */
|
||||||
|
if (dump_one) {
|
||||||
|
dump_one = false;
|
||||||
|
|
||||||
|
printf("IO: %s armed ", rep->armed ? "" : "not");
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < rep->channel_count; i++)
|
||||||
|
printf("%d: %d ", i, rep->rc_channel[i]);
|
||||||
|
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
|
||||||
out:
|
out:
|
||||||
unlock();
|
// unlock();
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -491,18 +613,29 @@ PX4IO::io_send()
|
||||||
cmd.f2i_magic = F2I_MAGIC;
|
cmd.f2i_magic = F2I_MAGIC;
|
||||||
|
|
||||||
/* set outputs */
|
/* set outputs */
|
||||||
for (unsigned i = 0; i < _max_actuators; i++)
|
for (unsigned i = 0; i < _max_actuators; i++) {
|
||||||
cmd.servo_command[i] = _outputs.output[i];
|
cmd.output_control[i] = _outputs.output[i];
|
||||||
|
}
|
||||||
/* publish as we send */
|
/* publish as we send */
|
||||||
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
|
_outputs.timestamp = hrt_absolute_time();
|
||||||
|
/* XXX needs to be based off post-mix values from the IO side */
|
||||||
|
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
|
||||||
|
|
||||||
// XXX relays
|
/* update relays */
|
||||||
|
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
|
||||||
|
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
|
||||||
|
|
||||||
/* armed and not locked down */
|
/* armed and not locked down -> arming ok */
|
||||||
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
|
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
|
||||||
|
/* indicate that full autonomous position control / vector flight mode is available */
|
||||||
|
cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
|
||||||
|
/* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
|
||||||
|
cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
|
||||||
|
/* set desired PWM output rate */
|
||||||
|
cmd.servo_rate = _update_rate;
|
||||||
|
|
||||||
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
|
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
|
||||||
|
|
||||||
if (ret)
|
if (ret)
|
||||||
debug("send error %d", ret);
|
debug("send error %d", ret);
|
||||||
}
|
}
|
||||||
|
@ -515,11 +648,88 @@ PX4IO::config_send()
|
||||||
|
|
||||||
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
|
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
|
||||||
|
|
||||||
|
int val;
|
||||||
|
|
||||||
|
/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
|
||||||
|
param_get(param_find("RC_MAP_ROLL"), &val);
|
||||||
|
cfg.rc_map[0] = val;
|
||||||
|
param_get(param_find("RC_MAP_PITCH"), &val);
|
||||||
|
cfg.rc_map[1] = val;
|
||||||
|
param_get(param_find("RC_MAP_YAW"), &val);
|
||||||
|
cfg.rc_map[2] = val;
|
||||||
|
param_get(param_find("RC_MAP_THROTTLE"), &val);
|
||||||
|
cfg.rc_map[3] = val;
|
||||||
|
|
||||||
|
/* set the individual channel properties */
|
||||||
|
char nbuf[16];
|
||||||
|
float float_val;
|
||||||
|
for (unsigned i = 0; i < 4; i++) {
|
||||||
|
sprintf(nbuf, "RC%d_MIN", i + 1);
|
||||||
|
param_get(param_find(nbuf), &float_val);
|
||||||
|
cfg.rc_min[i] = float_val;
|
||||||
|
}
|
||||||
|
for (unsigned i = 0; i < 4; i++) {
|
||||||
|
sprintf(nbuf, "RC%d_TRIM", i + 1);
|
||||||
|
param_get(param_find(nbuf), &float_val);
|
||||||
|
cfg.rc_trim[i] = float_val;
|
||||||
|
}
|
||||||
|
for (unsigned i = 0; i < 4; i++) {
|
||||||
|
sprintf(nbuf, "RC%d_MAX", i + 1);
|
||||||
|
param_get(param_find(nbuf), &float_val);
|
||||||
|
cfg.rc_max[i] = float_val;
|
||||||
|
}
|
||||||
|
for (unsigned i = 0; i < 4; i++) {
|
||||||
|
sprintf(nbuf, "RC%d_REV", i + 1);
|
||||||
|
param_get(param_find(nbuf), &float_val);
|
||||||
|
cfg.rc_rev[i] = float_val;
|
||||||
|
}
|
||||||
|
for (unsigned i = 0; i < 4; i++) {
|
||||||
|
sprintf(nbuf, "RC%d_DZ", i + 1);
|
||||||
|
param_get(param_find(nbuf), &float_val);
|
||||||
|
cfg.rc_dz[i] = float_val;
|
||||||
|
}
|
||||||
|
|
||||||
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
|
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
|
||||||
|
|
||||||
if (ret)
|
if (ret)
|
||||||
debug("config error %d", ret);
|
debug("config error %d", ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
PX4IO::mixer_send(const char *buf, unsigned buflen)
|
||||||
|
{
|
||||||
|
uint8_t frame[HX_STREAM_MAX_FRAME];
|
||||||
|
px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
|
||||||
|
|
||||||
|
msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
|
||||||
|
msg->action = F2I_MIXER_ACTION_RESET;
|
||||||
|
|
||||||
|
do {
|
||||||
|
unsigned count = buflen;
|
||||||
|
|
||||||
|
if (count > F2I_MIXER_MAX_TEXT)
|
||||||
|
count = F2I_MIXER_MAX_TEXT;
|
||||||
|
|
||||||
|
if (count > 0) {
|
||||||
|
memcpy(&msg->text[0], buf, count);
|
||||||
|
buf += count;
|
||||||
|
buflen -= count;
|
||||||
|
}
|
||||||
|
|
||||||
|
int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
|
||||||
|
|
||||||
|
if (ret) {
|
||||||
|
log("mixer send error %d", ret);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
msg->action = F2I_MIXER_ACTION_APPEND;
|
||||||
|
|
||||||
|
} while (buflen > 0);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
{
|
{
|
||||||
|
@ -541,9 +751,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
_send_needed = true;
|
_send_needed = true;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case PWM_SERVO_SET_UPDATE_RATE:
|
||||||
|
// not supported yet
|
||||||
|
ret = -EINVAL;
|
||||||
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
|
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
|
||||||
|
|
||||||
/* fake an update to the selected servo channel */
|
/* fake an update to the selected 'servo' channel */
|
||||||
if ((arg >= 900) && (arg <= 2100)) {
|
if ((arg >= 900) && (arg <= 2100)) {
|
||||||
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
|
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
|
||||||
_send_needed = true;
|
_send_needed = true;
|
||||||
|
@ -559,68 +774,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
|
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case GPIO_RESET:
|
||||||
|
_relays = 0;
|
||||||
|
_send_needed = true;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GPIO_SET:
|
||||||
|
case GPIO_CLEAR:
|
||||||
|
/* make sure only valid bits are being set */
|
||||||
|
if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
|
||||||
|
ret = EINVAL;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (cmd == GPIO_SET) {
|
||||||
|
_relays |= arg;
|
||||||
|
} else {
|
||||||
|
_relays &= ~arg;
|
||||||
|
}
|
||||||
|
_send_needed = true;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GPIO_GET:
|
||||||
|
*(uint32_t *)arg = _relays;
|
||||||
|
break;
|
||||||
|
|
||||||
case MIXERIOCGETOUTPUTCOUNT:
|
case MIXERIOCGETOUTPUTCOUNT:
|
||||||
*(unsigned *)arg = _max_actuators;
|
*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCRESET:
|
case MIXERIOCRESET:
|
||||||
if (_mixers != nullptr) {
|
ret = 0; /* load always resets */
|
||||||
delete _mixers;
|
|
||||||
_mixers = nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCADDSIMPLE: {
|
case MIXERIOCLOADBUF:
|
||||||
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
|
|
||||||
|
|
||||||
/* build the new mixer from the supplied argument */
|
/* set the buffer up for transfer */
|
||||||
SimpleMixer *mixer = new SimpleMixer(control_callback,
|
_mix_buf = (const char *)arg;
|
||||||
(uintptr_t)&_controls, mixinfo);
|
_mix_buf_len = strnlen(_mix_buf, 1024);
|
||||||
|
|
||||||
/* validate the new mixer */
|
/* drop the lock and wait for the thread to clear the transmit */
|
||||||
if (mixer->check()) {
|
unlock();
|
||||||
delete mixer;
|
|
||||||
ret = -EINVAL;
|
|
||||||
|
|
||||||
} else {
|
while (_mix_buf != nullptr)
|
||||||
/* if we don't have a group yet, allocate one */
|
usleep(1000);
|
||||||
if (_mixers == nullptr)
|
|
||||||
_mixers = new MixerGroup(control_callback,
|
|
||||||
(uintptr_t)&_controls);
|
|
||||||
|
|
||||||
/* add the new mixer to the group */
|
lock();
|
||||||
_mixers->add_mixer(mixer);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
ret = 0;
|
||||||
break;
|
|
||||||
|
|
||||||
case MIXERIOCADDMULTIROTOR:
|
|
||||||
/* XXX not yet supported */
|
|
||||||
ret = -ENOTTY;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MIXERIOCLOADFILE: {
|
|
||||||
MixerGroup *newmixers;
|
|
||||||
const char *path = (const char *)arg;
|
|
||||||
|
|
||||||
/* allocate a new mixer group and load it from the file */
|
|
||||||
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
|
||||||
|
|
||||||
if (newmixers->load_from_file(path) != 0) {
|
|
||||||
delete newmixers;
|
|
||||||
ret = -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* swap the new mixers in for the old */
|
|
||||||
if (_mixers != nullptr) {
|
|
||||||
delete _mixers;
|
|
||||||
}
|
|
||||||
|
|
||||||
_mixers = newmixers;
|
|
||||||
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -662,9 +862,42 @@ test(void)
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
|
actuator_armed_s aa;
|
||||||
|
|
||||||
|
aa.armed = true;
|
||||||
|
aa.lockdown = false;
|
||||||
|
|
||||||
|
orb_advertise(ORB_ID(actuator_armed), &aa);
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
monitor(void)
|
||||||
|
{
|
||||||
|
unsigned cancels = 3;
|
||||||
|
printf("Hit <enter> three times to exit monitor mode\n");
|
||||||
|
|
||||||
|
for (;;) {
|
||||||
|
pollfd fds[1];
|
||||||
|
|
||||||
|
fds[0].fd = 0;
|
||||||
|
fds[0].events = POLLIN;
|
||||||
|
poll(fds, 1, 500);
|
||||||
|
|
||||||
|
if (fds[0].revents == POLLIN) {
|
||||||
|
int c;
|
||||||
|
read(0, &c, 1);
|
||||||
|
|
||||||
|
if (cancels-- == 0)
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (g_dev != nullptr)
|
||||||
|
g_dev->dump_one = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -686,12 +919,51 @@ px4io_main(int argc, char *argv[])
|
||||||
errx(1, "driver init failed");
|
errx(1, "driver init failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* look for the optional pwm update rate for the supported modes */
|
||||||
|
if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
|
||||||
|
if (argc > 2 + 1) {
|
||||||
|
g_dev->set_pwm_rate(atoi(argv[2 + 1]));
|
||||||
|
} else {
|
||||||
|
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "stop")) {
|
||||||
|
|
||||||
|
if (g_dev != nullptr) {
|
||||||
|
/* stop the driver */
|
||||||
|
delete g_dev;
|
||||||
|
} else {
|
||||||
|
errx(1, "not loaded");
|
||||||
|
}
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "status")) {
|
||||||
|
|
||||||
|
if (g_dev != nullptr)
|
||||||
|
printf("[px4io] loaded\n");
|
||||||
|
else
|
||||||
|
printf("[px4io] not loaded\n");
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
/* note, stop not currently implemented */
|
/* note, stop not currently implemented */
|
||||||
|
|
||||||
if (!strcmp(argv[1], "update")) {
|
if (!strcmp(argv[1], "update")) {
|
||||||
|
|
||||||
|
if (g_dev != nullptr) {
|
||||||
|
printf("[px4io] loaded, detaching first\n");
|
||||||
|
/* stop the driver */
|
||||||
|
delete g_dev;
|
||||||
|
}
|
||||||
|
|
||||||
PX4IO_Uploader *up;
|
PX4IO_Uploader *up;
|
||||||
const char *fn[3];
|
const char *fn[3];
|
||||||
|
|
||||||
|
@ -740,8 +1012,12 @@ px4io_main(int argc, char *argv[])
|
||||||
!strcmp(argv[1], "rx_sbus") ||
|
!strcmp(argv[1], "rx_sbus") ||
|
||||||
!strcmp(argv[1], "rx_ppm"))
|
!strcmp(argv[1], "rx_ppm"))
|
||||||
errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
|
errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
|
||||||
|
|
||||||
if (!strcmp(argv[1], "test"))
|
if (!strcmp(argv[1], "test"))
|
||||||
test();
|
test();
|
||||||
|
|
||||||
errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
|
if (!strcmp(argv[1], "monitor"))
|
||||||
|
monitor();
|
||||||
|
|
||||||
|
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'");
|
||||||
}
|
}
|
||||||
|
|
|
@ -51,6 +51,50 @@
|
||||||
|
|
||||||
#include "uploader.h"
|
#include "uploader.h"
|
||||||
|
|
||||||
|
static const uint32_t crctab[] =
|
||||||
|
{
|
||||||
|
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
|
||||||
|
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
|
||||||
|
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
|
||||||
|
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
|
||||||
|
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
|
||||||
|
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
|
||||||
|
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
|
||||||
|
0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
|
||||||
|
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
|
||||||
|
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
|
||||||
|
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
|
||||||
|
0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
|
||||||
|
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
|
||||||
|
0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
|
||||||
|
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
|
||||||
|
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
|
||||||
|
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
|
||||||
|
0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
|
||||||
|
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
|
||||||
|
0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
|
||||||
|
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
|
||||||
|
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
|
||||||
|
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
|
||||||
|
0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
|
||||||
|
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
|
||||||
|
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
|
||||||
|
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
|
||||||
|
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
|
||||||
|
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
|
||||||
|
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
|
||||||
|
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
|
||||||
|
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
|
||||||
|
};
|
||||||
|
|
||||||
|
static uint32_t
|
||||||
|
crc32(const uint8_t *src, unsigned len, unsigned state)
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < len; i++)
|
||||||
|
state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
PX4IO_Uploader::PX4IO_Uploader() :
|
PX4IO_Uploader::PX4IO_Uploader() :
|
||||||
_io_fd(-1),
|
_io_fd(-1),
|
||||||
_fw_fd(-1)
|
_fw_fd(-1)
|
||||||
|
@ -110,6 +154,17 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
ret = get_info(INFO_BL_REV, bl_rev);
|
||||||
|
|
||||||
|
if (ret == OK) {
|
||||||
|
if (bl_rev <= BL_REV) {
|
||||||
|
log("found bootloader revision: %d", bl_rev);
|
||||||
|
} else {
|
||||||
|
log("found unsupported bootloader revision %d, exiting", bl_rev);
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
ret = erase();
|
ret = erase();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
|
@ -124,7 +179,11 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = verify();
|
if (bl_rev <= 2)
|
||||||
|
ret = verify_rev2();
|
||||||
|
else if(bl_rev == 3) {
|
||||||
|
ret = verify_rev3();
|
||||||
|
}
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("verify failed");
|
log("verify failed");
|
||||||
|
@ -190,6 +249,7 @@ PX4IO_Uploader::drain()
|
||||||
|
|
||||||
do {
|
do {
|
||||||
ret = recv(c, 250);
|
ret = recv(c, 250);
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
//log("discard 0x%02x", c);
|
//log("discard 0x%02x", c);
|
||||||
}
|
}
|
||||||
|
@ -319,7 +379,7 @@ PX4IO_Uploader::program()
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
PX4IO_Uploader::verify()
|
PX4IO_Uploader::verify_rev2()
|
||||||
{
|
{
|
||||||
uint8_t file_buf[PROG_MULTI_MAX];
|
uint8_t file_buf[PROG_MULTI_MAX];
|
||||||
ssize_t count;
|
ssize_t count;
|
||||||
|
@ -379,6 +439,74 @@ PX4IO_Uploader::verify()
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
PX4IO_Uploader::verify_rev3()
|
||||||
|
{
|
||||||
|
int ret;
|
||||||
|
uint8_t file_buf[4];
|
||||||
|
ssize_t count;
|
||||||
|
uint32_t sum = 0;
|
||||||
|
uint32_t bytes_read = 0;
|
||||||
|
uint32_t fw_size = 0;
|
||||||
|
uint32_t crc = 0;
|
||||||
|
uint8_t fill_blank = 0xff;
|
||||||
|
|
||||||
|
log("verify...");
|
||||||
|
lseek(_fw_fd, 0, SEEK_SET);
|
||||||
|
|
||||||
|
ret = get_info(INFO_FLASH_SIZE, fw_size);
|
||||||
|
send(PROTO_EOC);
|
||||||
|
|
||||||
|
if (ret != OK) {
|
||||||
|
log("could not read firmware size");
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* read through the firmware file again and calculate the checksum*/
|
||||||
|
while (true) {
|
||||||
|
lseek(_fw_fd, 0, SEEK_CUR);
|
||||||
|
count = read(_fw_fd, file_buf, sizeof(file_buf));
|
||||||
|
|
||||||
|
/* set the rest to ff */
|
||||||
|
if (count == 0) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
/* stop if the file cannot be read */
|
||||||
|
if (count < 0)
|
||||||
|
return -errno;
|
||||||
|
|
||||||
|
/* calculate crc32 sum */
|
||||||
|
sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
|
||||||
|
|
||||||
|
bytes_read += count;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* fill the rest with 0xff */
|
||||||
|
while (bytes_read < fw_size) {
|
||||||
|
sum = crc32(&fill_blank, sizeof(fill_blank), sum);
|
||||||
|
bytes_read += sizeof(fill_blank);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* request CRC from IO */
|
||||||
|
send(PROTO_GET_CRC);
|
||||||
|
send(PROTO_EOC);
|
||||||
|
|
||||||
|
ret = recv((uint8_t*)(&crc), sizeof(crc));
|
||||||
|
|
||||||
|
if (ret != OK) {
|
||||||
|
log("did not receive CRC checksum");
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* compare the CRC sum from the IO with the one calculated */
|
||||||
|
if (sum != crc) {
|
||||||
|
log("CRC wrong: received: %d, expected: %d", crc, sum);
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
PX4IO_Uploader::reboot()
|
PX4IO_Uploader::reboot()
|
||||||
{
|
{
|
||||||
|
|
|
@ -64,21 +64,25 @@ private:
|
||||||
PROTO_CHIP_VERIFY = 0x24,
|
PROTO_CHIP_VERIFY = 0x24,
|
||||||
PROTO_PROG_MULTI = 0x27,
|
PROTO_PROG_MULTI = 0x27,
|
||||||
PROTO_READ_MULTI = 0x28,
|
PROTO_READ_MULTI = 0x28,
|
||||||
|
PROTO_GET_CRC = 0x29,
|
||||||
PROTO_REBOOT = 0x30,
|
PROTO_REBOOT = 0x30,
|
||||||
|
|
||||||
INFO_BL_REV = 1, /**< bootloader protocol revision */
|
INFO_BL_REV = 1, /**< bootloader protocol revision */
|
||||||
BL_REV = 2, /**< supported bootloader protocol */
|
BL_REV = 3, /**< supported bootloader protocol */
|
||||||
INFO_BOARD_ID = 2, /**< board type */
|
INFO_BOARD_ID = 2, /**< board type */
|
||||||
INFO_BOARD_REV = 3, /**< board revision */
|
INFO_BOARD_REV = 3, /**< board revision */
|
||||||
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
|
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
|
||||||
|
|
||||||
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
|
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
|
||||||
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
|
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
int _io_fd;
|
int _io_fd;
|
||||||
int _fw_fd;
|
int _fw_fd;
|
||||||
|
|
||||||
|
uint32_t bl_rev; /**< bootloader revision */
|
||||||
|
|
||||||
void log(const char *fmt, ...);
|
void log(const char *fmt, ...);
|
||||||
|
|
||||||
int recv(uint8_t &c, unsigned timeout = 1000);
|
int recv(uint8_t &c, unsigned timeout = 1000);
|
||||||
|
@ -91,7 +95,8 @@ private:
|
||||||
int get_info(int param, uint32_t &val);
|
int get_info(int param, uint32_t &val);
|
||||||
int erase();
|
int erase();
|
||||||
int program();
|
int program();
|
||||||
int verify();
|
int verify_rev2();
|
||||||
|
int verify_rev3();
|
||||||
int reboot();
|
int reboot();
|
||||||
int compare(bool &identical);
|
int compare(bool &identical);
|
||||||
};
|
};
|
||||||
|
|
|
@ -0,0 +1,43 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# STM32 ADC driver
|
||||||
|
#
|
||||||
|
|
||||||
|
APPNAME = adc
|
||||||
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
|
STACKSIZE = 2048
|
||||||
|
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,387 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 adc.cpp
|
||||||
|
*
|
||||||
|
* Driver for the STM32 ADC.
|
||||||
|
*
|
||||||
|
* This is a low-rate driver, designed for sampling things like voltages
|
||||||
|
* and so forth. It avoids the gross complexity of the NuttX ADC driver.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <drivers/device/device.h>
|
||||||
|
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
#include <arch/board/board.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <drivers/drv_adc.h>
|
||||||
|
|
||||||
|
#include <arch/stm32/chip.h>
|
||||||
|
#include <stm32_internal.h>
|
||||||
|
#include <stm32_gpio.h>
|
||||||
|
|
||||||
|
#include <systemlib/err.h>
|
||||||
|
#include <systemlib/perf_counter.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Register accessors.
|
||||||
|
* For now, no reason not to just use ADC1.
|
||||||
|
*/
|
||||||
|
#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
|
||||||
|
|
||||||
|
#define rSR REG(STM32_ADC_SR_OFFSET)
|
||||||
|
#define rCR1 REG(STM32_ADC_CR1_OFFSET)
|
||||||
|
#define rCR2 REG(STM32_ADC_CR2_OFFSET)
|
||||||
|
#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
|
||||||
|
#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
|
||||||
|
#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
|
||||||
|
#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
|
||||||
|
#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
|
||||||
|
#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
|
||||||
|
#define rHTR REG(STM32_ADC_HTR_OFFSET)
|
||||||
|
#define rLTR REG(STM32_ADC_LTR_OFFSET)
|
||||||
|
#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
|
||||||
|
#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
|
||||||
|
#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
|
||||||
|
#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
|
||||||
|
#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
|
||||||
|
#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
|
||||||
|
#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
|
||||||
|
#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
|
||||||
|
#define rDR REG(STM32_ADC_DR_OFFSET)
|
||||||
|
|
||||||
|
#ifdef STM32_ADC_CCR
|
||||||
|
# define rCCR REG(STM32_ADC_CCR_OFFSET)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
class ADC : public device::CDev
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ADC(uint32_t channels);
|
||||||
|
~ADC();
|
||||||
|
|
||||||
|
virtual int init();
|
||||||
|
|
||||||
|
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||||
|
virtual ssize_t read(file *filp, char *buffer, size_t len);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
virtual int open_first(struct file *filp);
|
||||||
|
virtual int close_last(struct file *filp);
|
||||||
|
|
||||||
|
private:
|
||||||
|
static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */
|
||||||
|
|
||||||
|
hrt_call _call;
|
||||||
|
perf_counter_t _sample_perf;
|
||||||
|
|
||||||
|
unsigned _channel_count;
|
||||||
|
adc_msg_s *_samples; /**< sample buffer */
|
||||||
|
|
||||||
|
/** work trampoline */
|
||||||
|
static void _tick_trampoline(void *arg);
|
||||||
|
|
||||||
|
/** worker function */
|
||||||
|
void _tick();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Sample a single channel and return the measured value.
|
||||||
|
*
|
||||||
|
* @param channel The channel to sample.
|
||||||
|
* @return The sampled value, or 0xffff if
|
||||||
|
* sampling failed.
|
||||||
|
*/
|
||||||
|
uint16_t _sample(unsigned channel);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
ADC::ADC(uint32_t channels) :
|
||||||
|
CDev("adc", ADC_DEVICE_PATH),
|
||||||
|
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
|
||||||
|
_channel_count(0),
|
||||||
|
_samples(nullptr)
|
||||||
|
{
|
||||||
|
_debug_enabled = true;
|
||||||
|
|
||||||
|
/* always enable the temperature sensor */
|
||||||
|
channels |= 1 << 16;
|
||||||
|
|
||||||
|
/* allocate the sample array */
|
||||||
|
for (unsigned i = 0; i < 32; i++) {
|
||||||
|
if (channels & (1 << i)) {
|
||||||
|
_channel_count++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
_samples = new adc_msg_s[_channel_count];
|
||||||
|
|
||||||
|
/* prefill the channel numbers in the sample array */
|
||||||
|
if (_samples != nullptr) {
|
||||||
|
unsigned index = 0;
|
||||||
|
for (unsigned i = 0; i < 32; i++) {
|
||||||
|
if (channels & (1 << i)) {
|
||||||
|
_samples[index].am_channel = i;
|
||||||
|
_samples[index].am_data = 0;
|
||||||
|
index++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ADC::~ADC()
|
||||||
|
{
|
||||||
|
if (_samples != nullptr)
|
||||||
|
delete _samples;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
ADC::init()
|
||||||
|
{
|
||||||
|
/* do calibration if supported */
|
||||||
|
#ifdef ADC_CR2_CAL
|
||||||
|
rCR2 |= ADC_CR2_CAL;
|
||||||
|
usleep(100);
|
||||||
|
if (rCR2 & ADC_CR2_CAL)
|
||||||
|
return -1;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* arbitrarily configure all channels for 55 cycle sample time */
|
||||||
|
rSMPR1 = 0b00000011011011011011011011011011;
|
||||||
|
rSMPR2 = 0b00011011011011011011011011011011;
|
||||||
|
|
||||||
|
/* XXX for F2/4, might want to select 12-bit mode? */
|
||||||
|
rCR1 = 0;
|
||||||
|
|
||||||
|
/* enable the temperature sensor / Vrefint channel if supported*/
|
||||||
|
rCR2 =
|
||||||
|
#ifdef ADC_CR2_TSVREFE
|
||||||
|
/* enable the temperature sensor in CR2 */
|
||||||
|
ADC_CR2_TSVREFE |
|
||||||
|
#endif
|
||||||
|
0;
|
||||||
|
|
||||||
|
#ifdef ADC_CCR_TSVREFE
|
||||||
|
/* enable temperature sensor in CCR */
|
||||||
|
rCCR = ADC_CCR_TSVREFE;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* configure for a single-channel sequence */
|
||||||
|
rSQR1 = 0;
|
||||||
|
rSQR2 = 0;
|
||||||
|
rSQR3 = 0; /* will be updated with the channel each tick */
|
||||||
|
|
||||||
|
/* power-cycle the ADC and turn it on */
|
||||||
|
rCR2 &= ~ADC_CR2_ADON;
|
||||||
|
usleep(10);
|
||||||
|
rCR2 |= ADC_CR2_ADON;
|
||||||
|
usleep(10);
|
||||||
|
rCR2 |= ADC_CR2_ADON;
|
||||||
|
usleep(10);
|
||||||
|
|
||||||
|
/* kick off a sample and wait for it to complete */
|
||||||
|
hrt_abstime now = hrt_absolute_time();
|
||||||
|
rCR2 |= ADC_CR2_SWSTART;
|
||||||
|
while (!(rSR & ADC_SR_EOC)) {
|
||||||
|
|
||||||
|
/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
|
||||||
|
if ((hrt_absolute_time() - now) > 500) {
|
||||||
|
log("sample timeout");
|
||||||
|
return -1;
|
||||||
|
return 0xffff;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
debug("init done");
|
||||||
|
|
||||||
|
/* create the device node */
|
||||||
|
return CDev::init();
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
ADC::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
|
{
|
||||||
|
return -ENOTTY;
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t
|
||||||
|
ADC::read(file *filp, char *buffer, size_t len)
|
||||||
|
{
|
||||||
|
const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
|
||||||
|
|
||||||
|
if (len > maxsize)
|
||||||
|
len = maxsize;
|
||||||
|
|
||||||
|
/* block interrupts while copying samples to avoid racing with an update */
|
||||||
|
irqstate_t flags = irqsave();
|
||||||
|
memcpy(buffer, _samples, len);
|
||||||
|
irqrestore(flags);
|
||||||
|
|
||||||
|
return len;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
ADC::open_first(struct file *filp)
|
||||||
|
{
|
||||||
|
/* get fresh data */
|
||||||
|
_tick();
|
||||||
|
|
||||||
|
/* and schedule regular updates */
|
||||||
|
hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
ADC::close_last(struct file *filp)
|
||||||
|
{
|
||||||
|
hrt_cancel(&_call);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
ADC::_tick_trampoline(void *arg)
|
||||||
|
{
|
||||||
|
((ADC *)arg)->_tick();
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
ADC::_tick()
|
||||||
|
{
|
||||||
|
/* scan the channel set and sample each */
|
||||||
|
for (unsigned i = 0; i < _channel_count; i++)
|
||||||
|
_samples[i].am_data = _sample(_samples[i].am_channel);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t
|
||||||
|
ADC::_sample(unsigned channel)
|
||||||
|
{
|
||||||
|
perf_begin(_sample_perf);
|
||||||
|
|
||||||
|
/* clear any previous EOC */
|
||||||
|
if (rSR & ADC_SR_EOC)
|
||||||
|
rSR &= ~ADC_SR_EOC;
|
||||||
|
|
||||||
|
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
|
||||||
|
rSQR3 = channel;
|
||||||
|
rCR2 |= ADC_CR2_SWSTART;
|
||||||
|
|
||||||
|
/* wait for the conversion to complete */
|
||||||
|
hrt_abstime now = hrt_absolute_time();
|
||||||
|
while (!(rSR & ADC_SR_EOC)) {
|
||||||
|
|
||||||
|
/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
|
||||||
|
if ((hrt_absolute_time() - now) > 50) {
|
||||||
|
log("sample timeout");
|
||||||
|
return 0xffff;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* read the result and clear EOC */
|
||||||
|
uint16_t result = rDR;
|
||||||
|
|
||||||
|
perf_end(_sample_perf);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Driver 'main' command.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int adc_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
ADC *g_adc;
|
||||||
|
|
||||||
|
void
|
||||||
|
test(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
int fd = open(ADC_DEVICE_PATH, O_RDONLY);
|
||||||
|
if (fd < 0)
|
||||||
|
err(1, "can't open ADC device");
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < 50; i++) {
|
||||||
|
adc_msg_s data[8];
|
||||||
|
ssize_t count = read(fd, data, sizeof(data));
|
||||||
|
|
||||||
|
if (count < 0)
|
||||||
|
errx(1, "read error");
|
||||||
|
|
||||||
|
unsigned channels = count / sizeof(data[0]);
|
||||||
|
|
||||||
|
for (unsigned j = 0; j < channels; j++) {
|
||||||
|
printf ("%d: %u ", data[j].am_channel, data[j].am_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("\n");
|
||||||
|
usleep(500000);
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
adc_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
if (g_adc == nullptr) {
|
||||||
|
/* XXX this hardcodes the minimum channel set for PX4FMU - should be configurable */
|
||||||
|
g_adc = new ADC((1 << 10) | (1 << 11));
|
||||||
|
|
||||||
|
if (g_adc == nullptr)
|
||||||
|
errx(1, "couldn't allocate the ADC driver");
|
||||||
|
|
||||||
|
if (g_adc->init() != OK) {
|
||||||
|
delete g_adc;
|
||||||
|
errx(1, "ADC init failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (argc > 1) {
|
||||||
|
if (!strcmp(argv[1], "test"))
|
||||||
|
test();
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
|
@ -60,13 +60,13 @@
|
||||||
|
|
||||||
#include "drv_pwm_servo.h"
|
#include "drv_pwm_servo.h"
|
||||||
|
|
||||||
#include "chip.h"
|
#include <chip.h>
|
||||||
#include "up_internal.h"
|
#include <up_internal.h>
|
||||||
#include "up_arch.h"
|
#include <up_arch.h>
|
||||||
|
|
||||||
#include "stm32_internal.h"
|
#include <stm32_internal.h>
|
||||||
#include "stm32_gpio.h"
|
#include <stm32_gpio.h>
|
||||||
#include "stm32_tim.h"
|
#include <stm32_tim.h>
|
||||||
|
|
||||||
|
|
||||||
/* default rate (in Hz) of PWM updates */
|
/* default rate (in Hz) of PWM updates */
|
||||||
|
@ -143,27 +143,27 @@ pwm_channel_init(unsigned channel)
|
||||||
/* configure the channel */
|
/* configure the channel */
|
||||||
switch (pwm_channels[channel].timer_channel) {
|
switch (pwm_channels[channel].timer_channel) {
|
||||||
case 1:
|
case 1:
|
||||||
rCCMR1(timer) |= (6 << 4);
|
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
|
||||||
rCCR1(timer) = pwm_channels[channel].default_value;
|
rCCR1(timer) = pwm_channels[channel].default_value;
|
||||||
rCCER(timer) |= (1 << 0);
|
rCCER(timer) |= GTIM_CCER_CC1E;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
rCCMR1(timer) |= (6 << 12);
|
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
|
||||||
rCCR2(timer) = pwm_channels[channel].default_value;
|
rCCR2(timer) = pwm_channels[channel].default_value;
|
||||||
rCCER(timer) |= (1 << 4);
|
rCCER(timer) |= GTIM_CCER_CC2E;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 3:
|
case 3:
|
||||||
rCCMR2(timer) |= (6 << 4);
|
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
|
||||||
rCCR3(timer) = pwm_channels[channel].default_value;
|
rCCR3(timer) = pwm_channels[channel].default_value;
|
||||||
rCCER(timer) |= (1 << 8);
|
rCCER(timer) |= GTIM_CCER_CC3E;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 4:
|
case 4:
|
||||||
rCCMR2(timer) |= (6 << 12);
|
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
|
||||||
rCCR4(timer) = pwm_channels[channel].default_value;
|
rCCR4(timer) = pwm_channels[channel].default_value;
|
||||||
rCCER(timer) |= (1 << 12);
|
rCCER(timer) |= GTIM_CCER_CC4E;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -288,17 +288,20 @@ up_pwm_servo_set_rate(unsigned rate)
|
||||||
void
|
void
|
||||||
up_pwm_servo_arm(bool armed)
|
up_pwm_servo_arm(bool armed)
|
||||||
{
|
{
|
||||||
/*
|
|
||||||
* XXX this is inelgant and in particular will either jam outputs at whatever level
|
|
||||||
* they happen to be at at the time the timers stop or generate runts.
|
|
||||||
* The right thing is almost certainly to kill auto-reload on the timers so that
|
|
||||||
* they just stop at the end of their count for disable, and to reset/restart them
|
|
||||||
* for enable.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* iterate timers and arm/disarm appropriately */
|
/* iterate timers and arm/disarm appropriately */
|
||||||
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
|
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
|
||||||
if (pwm_timers[i].base != 0)
|
if (pwm_timers[i].base != 0) {
|
||||||
rCR1(i) = armed ? GTIM_CR1_CEN : 0;
|
if (armed) {
|
||||||
|
/* force an update to preload all registers */
|
||||||
|
rEGR(i) = GTIM_EGR_UG;
|
||||||
|
|
||||||
|
/* arm requires the timer be enabled */
|
||||||
|
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
/* on disarm, just stop auto-reload so we don't generate runts */
|
||||||
|
rCR1(i) &= ~GTIM_CR1_ARPE;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -32,13 +32,11 @@
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
#
|
#
|
||||||
# fixedwing_control Application
|
# Basic example application
|
||||||
#
|
#
|
||||||
|
|
||||||
APPNAME = fixedwing_control
|
APPNAME = control_demo
|
||||||
PRIORITY = SCHED_PRIORITY_MAX - 1
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
STACKSIZE = 4096
|
STACKSIZE = 2048
|
||||||
|
|
||||||
CSRCS = fixedwing_control.c
|
|
||||||
|
|
||||||
include $(APPDIR)/mk/app.mk
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,168 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
* Author: @author Example User <mail@example.com>
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file control_demo.cpp
|
||||||
|
* Demonstration of control library
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <controllib/fixedwing.hpp>
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||||
|
static bool thread_running = false; /**< Deamon status flag */
|
||||||
|
static int deamon_task; /**< Handle of deamon task / thread */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deamon management function.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Mainloop of deamon.
|
||||||
|
*/
|
||||||
|
int control_demo_thread_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Test function
|
||||||
|
*/
|
||||||
|
void test();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Print the correct usage.
|
||||||
|
*/
|
||||||
|
static void usage(const char *reason);
|
||||||
|
|
||||||
|
static void
|
||||||
|
usage(const char *reason)
|
||||||
|
{
|
||||||
|
if (reason)
|
||||||
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
|
||||||
|
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The 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_create().
|
||||||
|
*/
|
||||||
|
int control_demo_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (argc < 1)
|
||||||
|
usage("missing command");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
|
if (thread_running) {
|
||||||
|
printf("control_demo already running\n");
|
||||||
|
/* this is not an error */
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
thread_should_exit = false;
|
||||||
|
deamon_task = task_spawn("control_demo",
|
||||||
|
SCHED_DEFAULT,
|
||||||
|
SCHED_PRIORITY_MAX - 10,
|
||||||
|
4096,
|
||||||
|
control_demo_thread_main,
|
||||||
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "test")) {
|
||||||
|
test();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "stop")) {
|
||||||
|
thread_should_exit = true;
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "status")) {
|
||||||
|
if (thread_running) {
|
||||||
|
printf("\tcontrol_demo app is running\n");
|
||||||
|
|
||||||
|
} else {
|
||||||
|
printf("\tcontrol_demo app not started\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
usage("unrecognized command");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
int control_demo_thread_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
printf("[control_Demo] starting\n");
|
||||||
|
|
||||||
|
using namespace control;
|
||||||
|
|
||||||
|
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
|
||||||
|
|
||||||
|
thread_running = true;
|
||||||
|
|
||||||
|
while (!thread_should_exit) {
|
||||||
|
autopilot.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("[control_demo] exiting.\n");
|
||||||
|
|
||||||
|
thread_running = false;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void test()
|
||||||
|
{
|
||||||
|
printf("beginning control lib test\n");
|
||||||
|
control::basicBlocksTest();
|
||||||
|
}
|
|
@ -0,0 +1,63 @@
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
// currently tuned for easystar from arkhangar in HIL
|
||||||
|
//https://github.com/arktools/arkhangar
|
||||||
|
|
||||||
|
// 16 is max name length
|
||||||
|
|
||||||
|
// gyro low pass filter
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
|
||||||
|
|
||||||
|
// yaw washout
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
|
||||||
|
|
||||||
|
// stabilization mode
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
|
||||||
|
|
||||||
|
// psi -> phi -> p
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f); // heading 2 roll
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f); // roll to roll rate
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f); // roll limit
|
||||||
|
|
||||||
|
// velocity -> theta
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
|
||||||
|
|
||||||
|
|
||||||
|
// theta -> q
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
|
||||||
|
|
||||||
|
// h -> thr
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
|
||||||
|
|
||||||
|
// crosstrack
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
|
||||||
|
|
||||||
|
// speed command
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
|
||||||
|
|
||||||
|
// trim
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
|
|
@ -0,0 +1,627 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 KalmanNav.cpp
|
||||||
|
*
|
||||||
|
* kalman filter navigation code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <poll.h>
|
||||||
|
|
||||||
|
#include "KalmanNav.hpp"
|
||||||
|
|
||||||
|
// constants
|
||||||
|
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
|
||||||
|
static const float R = 6.371000e6f; // earth radius, m
|
||||||
|
static const float RSq = 4.0589641e13f; // radius squared
|
||||||
|
static const float g = 9.8f; // gravitational accel. m/s^2
|
||||||
|
|
||||||
|
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
// ekf matrices
|
||||||
|
F(9, 9),
|
||||||
|
G(9, 6),
|
||||||
|
P(9, 9),
|
||||||
|
V(6, 6),
|
||||||
|
// attitude measurement ekf matrices
|
||||||
|
HAtt(6, 9),
|
||||||
|
RAtt(6, 6),
|
||||||
|
// gps measurement ekf matrices
|
||||||
|
HGps(6, 9),
|
||||||
|
RGps(6, 6),
|
||||||
|
// attitude representations
|
||||||
|
C_nb(),
|
||||||
|
q(),
|
||||||
|
// subscriptions
|
||||||
|
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
|
||||||
|
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
|
||||||
|
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||||
|
// publications
|
||||||
|
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
|
||||||
|
_att(&getPublications(), ORB_ID(vehicle_attitude)),
|
||||||
|
// timestamps
|
||||||
|
_pubTimeStamp(hrt_absolute_time()),
|
||||||
|
_fastTimeStamp(hrt_absolute_time()),
|
||||||
|
_slowTimeStamp(hrt_absolute_time()),
|
||||||
|
_attTimeStamp(hrt_absolute_time()),
|
||||||
|
_outTimeStamp(hrt_absolute_time()),
|
||||||
|
// frame count
|
||||||
|
_navFrames(0),
|
||||||
|
// state
|
||||||
|
fN(0), fE(0), fD(0),
|
||||||
|
phi(0), theta(0), psi(0),
|
||||||
|
vN(0), vE(0), vD(0),
|
||||||
|
lat(0), lon(0), alt(0),
|
||||||
|
// parameters for ground station
|
||||||
|
_vGyro(this, "V_GYRO"),
|
||||||
|
_vAccel(this, "V_ACCEL"),
|
||||||
|
_rMag(this, "R_MAG"),
|
||||||
|
_rGpsV(this, "R_GPS_V"),
|
||||||
|
_rGpsGeo(this, "R_GPS_GEO"),
|
||||||
|
_rGpsAlt(this, "R_GPS_ALT"),
|
||||||
|
_rAccel(this, "R_ACCEL")
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// initial state covariance matrix
|
||||||
|
P = Matrix::identity(9) * 1.0f;
|
||||||
|
|
||||||
|
// wait for gps lock
|
||||||
|
while (1) {
|
||||||
|
updateSubscriptions();
|
||||||
|
|
||||||
|
if (_gps.fix_type > 2) break;
|
||||||
|
|
||||||
|
printf("[kalman_demo] waiting for gps lock\n");
|
||||||
|
usleep(1000000);
|
||||||
|
}
|
||||||
|
|
||||||
|
// initial state
|
||||||
|
phi = 0.0f;
|
||||||
|
theta = 0.0f;
|
||||||
|
psi = 0.0f;
|
||||||
|
vN = _gps.vel_n;
|
||||||
|
vE = _gps.vel_e;
|
||||||
|
vD = _gps.vel_d;
|
||||||
|
setLatDegE7(_gps.lat);
|
||||||
|
setLonDegE7(_gps.lon);
|
||||||
|
setAltE3(_gps.alt);
|
||||||
|
|
||||||
|
// initialize quaternions
|
||||||
|
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||||
|
|
||||||
|
// initialize dcm
|
||||||
|
C_nb = Dcm(q);
|
||||||
|
|
||||||
|
// initialize F to identity
|
||||||
|
F = Matrix::identity(9);
|
||||||
|
|
||||||
|
// HGps is constant
|
||||||
|
HGps(0, 3) = 1.0f;
|
||||||
|
HGps(1, 4) = 1.0f;
|
||||||
|
HGps(2, 5) = 1.0f;
|
||||||
|
HGps(3, 6) = 1.0f;
|
||||||
|
HGps(4, 7) = 1.0f;
|
||||||
|
HGps(5, 8) = 1.0f;
|
||||||
|
|
||||||
|
// initialize all parameters
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::update()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
struct pollfd fds[2];
|
||||||
|
fds[0].fd = _sensors.getHandle();
|
||||||
|
fds[0].events = POLLIN;
|
||||||
|
fds[1].fd = _param_update.getHandle();
|
||||||
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
|
// poll 20 milliseconds for new data
|
||||||
|
int ret = poll(fds, 2, 20);
|
||||||
|
|
||||||
|
// check return value
|
||||||
|
if (ret < 0) {
|
||||||
|
// XXX this is seriously bad - should be an emergency
|
||||||
|
return;
|
||||||
|
|
||||||
|
} else if (ret == 0) { // timeout
|
||||||
|
// run anyway
|
||||||
|
} else if (ret > 0) {
|
||||||
|
// update params when requested
|
||||||
|
if (fds[1].revents & POLLIN) {
|
||||||
|
printf("updating params\n");
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
// if no new sensor data, return
|
||||||
|
if (!(fds[0].revents & POLLIN)) return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get new timestamp
|
||||||
|
uint64_t newTimeStamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
// check updated subscriptions
|
||||||
|
bool gpsUpdate = _gps.updated();
|
||||||
|
|
||||||
|
// get new information from subscriptions
|
||||||
|
// this clears update flag
|
||||||
|
updateSubscriptions();
|
||||||
|
|
||||||
|
// count fast frames
|
||||||
|
_navFrames += 1;
|
||||||
|
|
||||||
|
// fast prediciton step
|
||||||
|
// note, using sensors timestamp so we can account
|
||||||
|
// for packet lag
|
||||||
|
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
|
||||||
|
_fastTimeStamp = _sensors.timestamp;
|
||||||
|
predictFast(dtFast);
|
||||||
|
|
||||||
|
// slow prediction step
|
||||||
|
float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
|
||||||
|
|
||||||
|
if (dtSlow > 1.0f / 200) { // 200 Hz
|
||||||
|
_slowTimeStamp = _sensors.timestamp;
|
||||||
|
predictSlow(dtSlow);
|
||||||
|
}
|
||||||
|
|
||||||
|
// gps correction step
|
||||||
|
if (gpsUpdate) {
|
||||||
|
correctGps();
|
||||||
|
}
|
||||||
|
|
||||||
|
// attitude correction step
|
||||||
|
if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
|
||||||
|
_attTimeStamp = _sensors.timestamp;
|
||||||
|
correctAtt();
|
||||||
|
}
|
||||||
|
|
||||||
|
// publication
|
||||||
|
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
|
||||||
|
_pubTimeStamp = newTimeStamp;
|
||||||
|
updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
// output
|
||||||
|
if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
|
||||||
|
_outTimeStamp = newTimeStamp;
|
||||||
|
printf("nav: %4d Hz\n", _navFrames);
|
||||||
|
_navFrames = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::updatePublications()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// position publication
|
||||||
|
_pos.timestamp = _pubTimeStamp;
|
||||||
|
_pos.time_gps_usec = _gps.timestamp;
|
||||||
|
_pos.valid = true;
|
||||||
|
_pos.lat = getLatDegE7();
|
||||||
|
_pos.lon = getLonDegE7();
|
||||||
|
_pos.alt = float(alt);
|
||||||
|
_pos.relative_alt = float(alt); // TODO, make relative
|
||||||
|
_pos.vx = vN;
|
||||||
|
_pos.vy = vE;
|
||||||
|
_pos.vz = vD;
|
||||||
|
_pos.hdg = psi;
|
||||||
|
|
||||||
|
// attitude publication
|
||||||
|
_att.timestamp = _pubTimeStamp;
|
||||||
|
_att.roll = phi;
|
||||||
|
_att.pitch = theta;
|
||||||
|
_att.yaw = psi;
|
||||||
|
_att.rollspeed = _sensors.gyro_rad_s[0];
|
||||||
|
_att.pitchspeed = _sensors.gyro_rad_s[1];
|
||||||
|
_att.yawspeed = _sensors.gyro_rad_s[2];
|
||||||
|
// TODO, add gyro offsets to filter
|
||||||
|
_att.rate_offsets[0] = 0.0f;
|
||||||
|
_att.rate_offsets[1] = 0.0f;
|
||||||
|
_att.rate_offsets[2] = 0.0f;
|
||||||
|
|
||||||
|
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||||
|
_att.R[i][j] = C_nb(i, j);
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
|
||||||
|
|
||||||
|
_att.R_valid = true;
|
||||||
|
_att.q_valid = true;
|
||||||
|
_att.counter = _navFrames;
|
||||||
|
|
||||||
|
// update publications
|
||||||
|
SuperBlock::updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::predictFast(float dt)
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
Vector3 w(_sensors.gyro_rad_s);
|
||||||
|
|
||||||
|
// attitude
|
||||||
|
q = q + q.derivative(w) * dt;
|
||||||
|
|
||||||
|
// renormalize quaternion if needed
|
||||||
|
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
|
||||||
|
q = q.unit();
|
||||||
|
}
|
||||||
|
|
||||||
|
// C_nb update
|
||||||
|
C_nb = Dcm(q);
|
||||||
|
|
||||||
|
// euler update
|
||||||
|
EulerAngles euler(C_nb);
|
||||||
|
phi = euler.getPhi();
|
||||||
|
theta = euler.getTheta();
|
||||||
|
psi = euler.getPsi();
|
||||||
|
|
||||||
|
// specific acceleration in nav frame
|
||||||
|
Vector3 accelB(_sensors.accelerometer_m_s2);
|
||||||
|
Vector3 accelN = C_nb * accelB;
|
||||||
|
fN = accelN(0);
|
||||||
|
fE = accelN(1);
|
||||||
|
fD = accelN(2);
|
||||||
|
|
||||||
|
// trig
|
||||||
|
float sinL = sinf(lat);
|
||||||
|
float cosL = cosf(lat);
|
||||||
|
|
||||||
|
// position update
|
||||||
|
// neglects angular deflections in local gravity
|
||||||
|
// see Titerton pg. 70
|
||||||
|
float LDot = vN / (R + float(alt));
|
||||||
|
float lDot = vE / (cosL * (R + float(alt)));
|
||||||
|
float vNDot = fN - vE * (2 * omega +
|
||||||
|
lDot) * sinL +
|
||||||
|
vD * LDot;
|
||||||
|
float vDDot = fD - vE * (2 * omega + lDot) * cosL -
|
||||||
|
vN * LDot + g;
|
||||||
|
float vEDot = fE + vN * (2 * omega + lDot) * sinL +
|
||||||
|
vDDot * (2 * omega * cosL);
|
||||||
|
|
||||||
|
// rectangular integration
|
||||||
|
vN += vNDot * dt;
|
||||||
|
vE += vEDot * dt;
|
||||||
|
vD += vDDot * dt;
|
||||||
|
lat += double(LDot * dt);
|
||||||
|
lon += double(lDot * dt);
|
||||||
|
alt += double(-vD * dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::predictSlow(float dt)
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// trig
|
||||||
|
float sinL = sinf(lat);
|
||||||
|
float cosL = cosf(lat);
|
||||||
|
float cosLSq = cosL * cosL;
|
||||||
|
float tanL = tanf(lat);
|
||||||
|
|
||||||
|
// F Matrix
|
||||||
|
// Titterton pg. 291
|
||||||
|
//
|
||||||
|
// difference from Jacobian
|
||||||
|
// multiplity by dt for all elements
|
||||||
|
// add 1.0 to diagonal elements
|
||||||
|
|
||||||
|
F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
|
||||||
|
F(0, 2) = (vN / R) * dt;
|
||||||
|
F(0, 4) = (1.0f / R) * dt;
|
||||||
|
F(0, 6) = (-omega * sinL) * dt;
|
||||||
|
F(0, 8) = (-vE / RSq) * dt;
|
||||||
|
|
||||||
|
F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
|
||||||
|
F(1, 2) = (omega * cosL + vE / R) * dt;
|
||||||
|
F(1, 3) = (-1.0f / R) * dt;
|
||||||
|
F(1, 8) = (vN / RSq) * dt;
|
||||||
|
|
||||||
|
F(2, 0) = (-vN / R) * dt;
|
||||||
|
F(2, 1) = (-omega * cosL - vE / R) * dt;
|
||||||
|
F(2, 4) = (-tanL / R) * dt;
|
||||||
|
F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
|
||||||
|
F(2, 8) = (vE * tanL / RSq) * dt;
|
||||||
|
|
||||||
|
F(3, 1) = (-fD) * dt;
|
||||||
|
F(3, 2) = (fE) * dt;
|
||||||
|
F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
|
||||||
|
F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
|
||||||
|
F(3, 5) = (vN / R) * dt;
|
||||||
|
F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
|
||||||
|
F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
|
||||||
|
|
||||||
|
F(4, 0) = (fD) * dt;
|
||||||
|
F(4, 2) = (-fN) * dt;
|
||||||
|
F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
|
||||||
|
F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
|
||||||
|
F(4, 5) = (2 * omega * cosL + vE / R) * dt;
|
||||||
|
F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
|
||||||
|
vN * vE / (R * cosLSq)) * dt;
|
||||||
|
F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
|
||||||
|
|
||||||
|
F(5, 0) = (-fE) * dt;
|
||||||
|
F(5, 1) = (fN) * dt;
|
||||||
|
F(5, 3) = (-2 * vN / R) * dt;
|
||||||
|
F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
|
||||||
|
F(5, 6) = (2 * omega * vE * sinL) * dt;
|
||||||
|
F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
|
||||||
|
|
||||||
|
F(6, 3) = (1 / R) * dt;
|
||||||
|
F(6, 8) = (-vN / RSq) * dt;
|
||||||
|
|
||||||
|
F(7, 4) = (1 / (R * cosL)) * dt;
|
||||||
|
F(7, 6) = (vE * tanL / (R * cosL)) * dt;
|
||||||
|
F(7, 8) = (-vE / (cosL * RSq)) * dt;
|
||||||
|
|
||||||
|
F(8, 5) = (-1) * dt;
|
||||||
|
|
||||||
|
// G Matrix
|
||||||
|
// Titterton pg. 291
|
||||||
|
G(0, 0) = -C_nb(0, 0) * dt;
|
||||||
|
G(0, 1) = -C_nb(0, 1) * dt;
|
||||||
|
G(0, 2) = -C_nb(0, 2) * dt;
|
||||||
|
G(1, 0) = -C_nb(1, 0) * dt;
|
||||||
|
G(1, 1) = -C_nb(1, 1) * dt;
|
||||||
|
G(1, 2) = -C_nb(1, 2) * dt;
|
||||||
|
G(2, 0) = -C_nb(2, 0) * dt;
|
||||||
|
G(2, 1) = -C_nb(2, 1) * dt;
|
||||||
|
G(2, 2) = -C_nb(2, 2) * dt;
|
||||||
|
|
||||||
|
G(3, 3) = C_nb(0, 0) * dt;
|
||||||
|
G(3, 4) = C_nb(0, 1) * dt;
|
||||||
|
G(3, 5) = C_nb(0, 2) * dt;
|
||||||
|
G(4, 3) = C_nb(1, 0) * dt;
|
||||||
|
G(4, 4) = C_nb(1, 1) * dt;
|
||||||
|
G(4, 5) = C_nb(1, 2) * dt;
|
||||||
|
G(5, 3) = C_nb(2, 0) * dt;
|
||||||
|
G(5, 4) = C_nb(2, 1) * dt;
|
||||||
|
G(5, 5) = C_nb(2, 2) * dt;
|
||||||
|
|
||||||
|
// predict equations for kalman filter
|
||||||
|
P = F * P * F.transpose() + G * V * G.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::correctAtt()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// trig
|
||||||
|
float cosPhi = cosf(phi);
|
||||||
|
float cosTheta = cosf(theta);
|
||||||
|
float cosPsi = cosf(psi);
|
||||||
|
float sinPhi = sinf(phi);
|
||||||
|
float sinTheta = sinf(theta);
|
||||||
|
float sinPsi = sinf(psi);
|
||||||
|
|
||||||
|
// mag measurement
|
||||||
|
Vector3 zMag(_sensors.magnetometer_ga);
|
||||||
|
zMag = zMag.unit();
|
||||||
|
|
||||||
|
// mag predicted measurement
|
||||||
|
// choosing some typical magnetic field properties,
|
||||||
|
// TODO dip/dec depend on lat/ lon/ time
|
||||||
|
static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
|
||||||
|
static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
|
||||||
|
float bN = cosf(dip) * cosf(dec);
|
||||||
|
float bE = cosf(dip) * sinf(dec);
|
||||||
|
float bD = sinf(dip);
|
||||||
|
Vector3 bNav(bN, bE, bD);
|
||||||
|
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
|
||||||
|
|
||||||
|
// accel measurement
|
||||||
|
Vector3 zAccel(_sensors.accelerometer_m_s2);
|
||||||
|
zAccel = zAccel.unit();
|
||||||
|
|
||||||
|
// accel predicted measurement
|
||||||
|
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit();
|
||||||
|
|
||||||
|
// combined measurement
|
||||||
|
Vector zAtt(6);
|
||||||
|
Vector zAttHat(6);
|
||||||
|
|
||||||
|
for (int i = 0; i < 3; i++) {
|
||||||
|
zAtt(i) = zMag(i);
|
||||||
|
zAtt(i + 3) = zAccel(i);
|
||||||
|
zAttHat(i) = zMagHat(i);
|
||||||
|
zAttHat(i + 3) = zAccelHat(i);
|
||||||
|
}
|
||||||
|
|
||||||
|
// HMag , HAtt (0-2,:)
|
||||||
|
float tmp1 =
|
||||||
|
cosPsi * cosTheta * bN +
|
||||||
|
sinPsi * cosTheta * bE -
|
||||||
|
sinTheta * bD;
|
||||||
|
HAtt(0, 1) = -(
|
||||||
|
cosPsi * sinTheta * bN +
|
||||||
|
sinPsi * sinTheta * bE +
|
||||||
|
cosTheta * bD
|
||||||
|
);
|
||||||
|
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
|
||||||
|
HAtt(1, 0) =
|
||||||
|
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
|
||||||
|
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
|
||||||
|
cosPhi * cosTheta * bD;
|
||||||
|
HAtt(1, 1) = sinPhi * tmp1;
|
||||||
|
HAtt(1, 2) = -(
|
||||||
|
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
|
||||||
|
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
|
||||||
|
);
|
||||||
|
HAtt(2, 0) = -(
|
||||||
|
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
|
||||||
|
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
|
||||||
|
(sinPhi * cosTheta) * bD
|
||||||
|
);
|
||||||
|
HAtt(2, 1) = cosPhi * tmp1;
|
||||||
|
HAtt(2, 2) = -(
|
||||||
|
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
|
||||||
|
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
|
||||||
|
);
|
||||||
|
|
||||||
|
// HAccel , HAtt (3-5,:)
|
||||||
|
HAtt(3, 1) = cosTheta;
|
||||||
|
HAtt(4, 0) = -cosPhi * cosTheta;
|
||||||
|
HAtt(4, 1) = sinPhi * sinTheta;
|
||||||
|
HAtt(5, 0) = sinPhi * cosTheta;
|
||||||
|
HAtt(5, 1) = cosPhi * sinTheta;
|
||||||
|
|
||||||
|
// compute correction
|
||||||
|
Vector y = zAtt - zAttHat; // residual
|
||||||
|
Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance
|
||||||
|
Matrix K = P * HAtt.transpose() * S.inverse();
|
||||||
|
Vector xCorrect = K * y;
|
||||||
|
|
||||||
|
// check correciton is sane
|
||||||
|
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||||
|
float val = xCorrect(i);
|
||||||
|
|
||||||
|
if (isnan(val) || isinf(val)) {
|
||||||
|
// abort correction and return
|
||||||
|
printf("[kalman_demo] numerical failure in att correction\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// correct state
|
||||||
|
phi += xCorrect(PHI);
|
||||||
|
theta += xCorrect(THETA);
|
||||||
|
psi += xCorrect(PSI);
|
||||||
|
|
||||||
|
// update state covariance
|
||||||
|
P = P - K * HAtt * P;
|
||||||
|
|
||||||
|
// fault detection
|
||||||
|
float beta = y.dot(S.inverse() * y);
|
||||||
|
printf("attitude: beta = %8.4f\n", (double)beta);
|
||||||
|
|
||||||
|
if (beta > 10.0f) {
|
||||||
|
//printf("fault in attitude: beta = %8.4f\n", (double)beta);
|
||||||
|
//printf("y:\n"); y.print();
|
||||||
|
}
|
||||||
|
|
||||||
|
// update quaternions from euler
|
||||||
|
// angle correction
|
||||||
|
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::correctGps()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
Vector y(6);
|
||||||
|
y(0) = _gps.vel_n - vN;
|
||||||
|
y(1) = _gps.vel_e - vE;
|
||||||
|
y(2) = _gps.vel_d - vD;
|
||||||
|
y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
|
||||||
|
y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
|
||||||
|
y(5) = double(_gps.alt) / 1.0e3 - alt;
|
||||||
|
|
||||||
|
// compute correction
|
||||||
|
Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
|
||||||
|
Matrix K = P * HGps.transpose() * S.inverse();
|
||||||
|
Vector xCorrect = K * y;
|
||||||
|
|
||||||
|
// check correction is sane
|
||||||
|
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||||
|
float val = xCorrect(i);
|
||||||
|
|
||||||
|
if (isnan(val) || isinf(val)) {
|
||||||
|
// abort correction and return
|
||||||
|
printf("[kalman_demo] numerical failure in gps correction\n");
|
||||||
|
// fallback to GPS
|
||||||
|
vN = _gps.vel_n;
|
||||||
|
vE = _gps.vel_e;
|
||||||
|
vD = _gps.vel_d;
|
||||||
|
setLatDegE7(_gps.lat);
|
||||||
|
setLonDegE7(_gps.lon);
|
||||||
|
setAltE3(_gps.alt);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// correct state
|
||||||
|
vN += xCorrect(VN);
|
||||||
|
vE += xCorrect(VE);
|
||||||
|
vD += xCorrect(VD);
|
||||||
|
lat += double(xCorrect(LAT));
|
||||||
|
lon += double(xCorrect(LON));
|
||||||
|
alt += double(xCorrect(ALT));
|
||||||
|
|
||||||
|
// update state covariance
|
||||||
|
P = P - K * HGps * P;
|
||||||
|
|
||||||
|
// fault detetcion
|
||||||
|
float beta = y.dot(S.inverse() * y);
|
||||||
|
printf("gps: beta = %8.4f\n", (double)beta);
|
||||||
|
|
||||||
|
if (beta > 100.0f) {
|
||||||
|
//printf("fault in gps: beta = %8.4f\n", (double)beta);
|
||||||
|
//printf("y:\n"); y.print();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::updateParams()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
using namespace control;
|
||||||
|
SuperBlock::updateParams();
|
||||||
|
|
||||||
|
// gyro noise
|
||||||
|
V(0, 0) = _vGyro.get(); // gyro x, rad/s
|
||||||
|
V(1, 1) = _vGyro.get(); // gyro y
|
||||||
|
V(2, 2) = _vGyro.get(); // gyro z
|
||||||
|
|
||||||
|
// accel noise
|
||||||
|
V(3, 3) = _vAccel.get(); // accel x, m/s^2
|
||||||
|
V(4, 4) = _vAccel.get(); // accel y
|
||||||
|
V(5, 5) = _vAccel.get(); // accel z
|
||||||
|
|
||||||
|
// magnetometer noise
|
||||||
|
RAtt(0, 0) = _rMag.get(); // normalized direction
|
||||||
|
RAtt(1, 1) = _rMag.get();
|
||||||
|
RAtt(2, 2) = _rMag.get();
|
||||||
|
|
||||||
|
// accelerometer noise
|
||||||
|
RAtt(3, 3) = _rAccel.get(); // normalized direction
|
||||||
|
RAtt(4, 4) = _rAccel.get();
|
||||||
|
RAtt(5, 5) = _rAccel.get();
|
||||||
|
|
||||||
|
// gps noise
|
||||||
|
RGps(0, 0) = _rGpsV.get(); // vn, m/s
|
||||||
|
RGps(1, 1) = _rGpsV.get(); // ve
|
||||||
|
RGps(2, 2) = _rGpsV.get(); // vd
|
||||||
|
RGps(3, 3) = _rGpsGeo.get(); // L, rad
|
||||||
|
RGps(4, 4) = _rGpsGeo.get(); // l, rad
|
||||||
|
RGps(5, 5) = _rGpsAlt.get(); // h, m
|
||||||
|
}
|
|
@ -0,0 +1,116 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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 KalmanNav.hpp
|
||||||
|
*
|
||||||
|
* kalman filter navigation code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
//#define MATRIX_ASSERT
|
||||||
|
//#define VECTOR_ASSERT
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
#include <controllib/blocks.hpp>
|
||||||
|
#include <controllib/block/BlockParam.hpp>
|
||||||
|
#include <controllib/block/UOrbSubscription.hpp>
|
||||||
|
#include <controllib/block/UOrbPublication.hpp>
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
|
#include <uORB/topics/sensor_combined.h>
|
||||||
|
#include <uORB/topics/vehicle_gps_position.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <poll.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
class KalmanNav : public control::SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
KalmanNav(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~KalmanNav() {};
|
||||||
|
void update();
|
||||||
|
virtual void updatePublications();
|
||||||
|
void predictFast(float dt);
|
||||||
|
void predictSlow(float dt);
|
||||||
|
void correctAtt();
|
||||||
|
void correctGps();
|
||||||
|
virtual void updateParams();
|
||||||
|
protected:
|
||||||
|
math::Matrix F;
|
||||||
|
math::Matrix G;
|
||||||
|
math::Matrix P;
|
||||||
|
math::Matrix V;
|
||||||
|
math::Matrix HAtt;
|
||||||
|
math::Matrix RAtt;
|
||||||
|
math::Matrix HGps;
|
||||||
|
math::Matrix RGps;
|
||||||
|
math::Dcm C_nb;
|
||||||
|
math::Quaternion q;
|
||||||
|
control::UOrbSubscription<sensor_combined_s> _sensors;
|
||||||
|
control::UOrbSubscription<vehicle_gps_position_s> _gps;
|
||||||
|
control::UOrbSubscription<parameter_update_s> _param_update;
|
||||||
|
control::UOrbPublication<vehicle_global_position_s> _pos;
|
||||||
|
control::UOrbPublication<vehicle_attitude_s> _att;
|
||||||
|
uint64_t _pubTimeStamp;
|
||||||
|
uint64_t _fastTimeStamp;
|
||||||
|
uint64_t _slowTimeStamp;
|
||||||
|
uint64_t _attTimeStamp;
|
||||||
|
uint64_t _outTimeStamp;
|
||||||
|
uint16_t _navFrames;
|
||||||
|
float fN, fE, fD;
|
||||||
|
// states
|
||||||
|
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
|
||||||
|
float phi, theta, psi;
|
||||||
|
float vN, vE, vD;
|
||||||
|
double lat, lon, alt;
|
||||||
|
control::BlockParam<float> _vGyro;
|
||||||
|
control::BlockParam<float> _vAccel;
|
||||||
|
control::BlockParam<float> _rMag;
|
||||||
|
control::BlockParam<float> _rGpsV;
|
||||||
|
control::BlockParam<float> _rGpsGeo;
|
||||||
|
control::BlockParam<float> _rGpsAlt;
|
||||||
|
control::BlockParam<float> _rAccel;
|
||||||
|
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
|
||||||
|
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||||
|
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
|
||||||
|
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||||
|
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
|
||||||
|
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
|
||||||
|
};
|
|
@ -0,0 +1,42 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Basic example application
|
||||||
|
#
|
||||||
|
|
||||||
|
APPNAME = kalman_demo
|
||||||
|
PRIORITY = SCHED_PRIORITY_MAX - 30
|
||||||
|
STACKSIZE = 2048
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,152 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
* Author: @author Example User <mail@example.com>
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file kalman_demo.cpp
|
||||||
|
* Demonstration of control library
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include "KalmanNav.hpp"
|
||||||
|
|
||||||
|
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||||
|
static bool thread_running = false; /**< Deamon status flag */
|
||||||
|
static int deamon_task; /**< Handle of deamon task / thread */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deamon management function.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int kalman_demo_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Mainloop of deamon.
|
||||||
|
*/
|
||||||
|
int kalman_demo_thread_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Print the correct usage.
|
||||||
|
*/
|
||||||
|
static void usage(const char *reason);
|
||||||
|
|
||||||
|
static void
|
||||||
|
usage(const char *reason)
|
||||||
|
{
|
||||||
|
if (reason)
|
||||||
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
|
||||||
|
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The 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_create().
|
||||||
|
*/
|
||||||
|
int kalman_demo_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (argc < 1)
|
||||||
|
usage("missing command");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
|
if (thread_running) {
|
||||||
|
printf("kalman_demo already running\n");
|
||||||
|
/* this is not an error */
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
thread_should_exit = false;
|
||||||
|
deamon_task = task_spawn("kalman_demo",
|
||||||
|
SCHED_DEFAULT,
|
||||||
|
SCHED_PRIORITY_MAX - 5,
|
||||||
|
4096,
|
||||||
|
kalman_demo_thread_main,
|
||||||
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "stop")) {
|
||||||
|
thread_should_exit = true;
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "status")) {
|
||||||
|
if (thread_running) {
|
||||||
|
printf("\tkalman_demo app is running\n");
|
||||||
|
|
||||||
|
} else {
|
||||||
|
printf("\tkalman_demo app not started\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
usage("unrecognized command");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
int kalman_demo_thread_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
printf("[kalman_demo] starting\n");
|
||||||
|
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
thread_running = true;
|
||||||
|
|
||||||
|
KalmanNav nav(NULL, "KF");
|
||||||
|
|
||||||
|
while (!thread_should_exit) {
|
||||||
|
nav.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("[kalman_demo] exiting.\n");
|
||||||
|
|
||||||
|
thread_running = false;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
|
@ -0,0 +1,10 @@
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||||
|
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
|
|
@ -0,0 +1,42 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Basic example application
|
||||||
|
#
|
||||||
|
|
||||||
|
APPNAME = math_demo
|
||||||
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
|
STACKSIZE = 8192
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,105 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
* Author: @author Example User <mail@example.com>
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file math_demo.cpp
|
||||||
|
* Demonstration of math library
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Management function.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Test function
|
||||||
|
*/
|
||||||
|
void test();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Print the correct usage.
|
||||||
|
*/
|
||||||
|
static void usage(const char *reason);
|
||||||
|
|
||||||
|
static void
|
||||||
|
usage(const char *reason)
|
||||||
|
{
|
||||||
|
if (reason)
|
||||||
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
fprintf(stderr, "usage: math_demo {test}\n\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The 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_create().
|
||||||
|
*/
|
||||||
|
int math_demo_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (argc < 1)
|
||||||
|
usage("missing command");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "test")) {
|
||||||
|
test();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
usage("unrecognized command");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void test()
|
||||||
|
{
|
||||||
|
printf("beginning math lib test\n");
|
||||||
|
using namespace math;
|
||||||
|
vectorTest();
|
||||||
|
matrixTest();
|
||||||
|
vector3Test();
|
||||||
|
eulerAnglesTest();
|
||||||
|
quaternionTest();
|
||||||
|
dcmTest();
|
||||||
|
}
|
|
@ -112,8 +112,9 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
|
||||||
}
|
}
|
||||||
|
|
||||||
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||||
const struct vehicle_attitude_s *att,
|
const struct vehicle_attitude_s *att,
|
||||||
struct vehicle_rates_setpoint_s *rates_sp)
|
const float speed_body[],
|
||||||
|
struct vehicle_rates_setpoint_s *rates_sp)
|
||||||
{
|
{
|
||||||
static int counter = 0;
|
static int counter = 0;
|
||||||
static bool initialized = false;
|
static bool initialized = false;
|
||||||
|
@ -125,8 +126,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
|
||||||
static PID_t pitch_controller;
|
static PID_t pitch_controller;
|
||||||
|
|
||||||
|
|
||||||
if(!initialized)
|
if (!initialized) {
|
||||||
{
|
|
||||||
parameters_init(&h);
|
parameters_init(&h);
|
||||||
parameters_update(&h, &p);
|
parameters_update(&h, &p);
|
||||||
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
|
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
|
||||||
|
@ -145,12 +145,21 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
|
||||||
/* Roll (P) */
|
/* Roll (P) */
|
||||||
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
|
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
|
||||||
|
|
||||||
|
|
||||||
/* Pitch (P) */
|
/* Pitch (P) */
|
||||||
float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body;
|
|
||||||
rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
|
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
|
||||||
|
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
|
||||||
|
/* set pitch plus feedforward roll compensation */
|
||||||
|
rates_sp->pitch = pid_calculate(&pitch_controller,
|
||||||
|
att_sp->pitch_body + pitch_sp_rollcompensation,
|
||||||
|
att->pitch, 0, 0);
|
||||||
|
|
||||||
/* Yaw (from coordinated turn constraint or lateral force) */
|
/* Yaw (from coordinated turn constraint or lateral force) */
|
||||||
//TODO
|
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
|
||||||
|
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
|
||||||
|
|
||||||
|
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
|
||||||
|
|
||||||
counter++;
|
counter++;
|
||||||
|
|
||||||
|
|
|
@ -41,9 +41,11 @@
|
||||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
|
|
||||||
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||||
const struct vehicle_attitude_s *att,
|
const struct vehicle_attitude_s *att,
|
||||||
struct vehicle_rates_setpoint_s *rates_sp);
|
const float speed_body[],
|
||||||
|
struct vehicle_rates_setpoint_s *rates_sp);
|
||||||
|
|
||||||
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
|
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
|
||||||
|
|
|
@ -58,40 +58,16 @@
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
#include <uORB/topics/debug_key_value.h>
|
#include <uORB/topics/debug_key_value.h>
|
||||||
#include <systemlib/param/param.h>
|
#include <systemlib/param/param.h>
|
||||||
#include <systemlib/pid/pid.h>
|
#include <systemlib/pid/pid.h>
|
||||||
#include <systemlib/geo/geo.h>
|
#include <systemlib/geo/geo.h>
|
||||||
|
#include <systemlib/perf_counter.h>
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
|
|
||||||
#include <fixedwing_att_control_rate.h>
|
#include <fixedwing_att_control_rate.h>
|
||||||
#include <fixedwing_att_control_att.h>
|
#include <fixedwing_att_control_att.h>
|
||||||
|
|
||||||
/*
|
|
||||||
* Controller parameters, accessible via MAVLink
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
// Roll control parameters
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
|
|
||||||
|
|
||||||
//Pitch control parameters
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
|
|
||||||
|
|
||||||
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
|
|
||||||
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
|
|
||||||
|
|
||||||
/* Prototypes */
|
/* Prototypes */
|
||||||
/**
|
/**
|
||||||
* Deamon management function.
|
* Deamon management function.
|
||||||
|
@ -117,118 +93,211 @@ static int deamon_task; /**< Handle of deamon task / thread */
|
||||||
int fixedwing_att_control_thread_main(int argc, char *argv[])
|
int fixedwing_att_control_thread_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
/* read arguments */
|
/* read arguments */
|
||||||
bool verbose = false;
|
bool verbose = false;
|
||||||
|
|
||||||
for (int i = 1; i < argc; i++) {
|
for (int i = 1; i < argc; i++) {
|
||||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||||
verbose = true;
|
verbose = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* welcome user */
|
||||||
|
printf("[fixedwing att control] started\n");
|
||||||
|
|
||||||
|
/* declare and safely initialize all structs */
|
||||||
|
struct vehicle_attitude_s att;
|
||||||
|
memset(&att, 0, sizeof(att));
|
||||||
|
struct vehicle_attitude_setpoint_s att_sp;
|
||||||
|
memset(&att_sp, 0, sizeof(att_sp));
|
||||||
|
struct vehicle_rates_setpoint_s rates_sp;
|
||||||
|
memset(&rates_sp, 0, sizeof(rates_sp));
|
||||||
|
struct vehicle_global_position_s global_pos;
|
||||||
|
memset(&global_pos, 0, sizeof(global_pos));
|
||||||
|
struct manual_control_setpoint_s manual_sp;
|
||||||
|
memset(&manual_sp, 0, sizeof(manual_sp));
|
||||||
|
struct vehicle_status_s vstatus;
|
||||||
|
memset(&vstatus, 0, sizeof(vstatus));
|
||||||
|
|
||||||
|
/* output structs */
|
||||||
|
struct actuator_controls_s actuators;
|
||||||
|
memset(&actuators, 0, sizeof(actuators));
|
||||||
|
|
||||||
|
|
||||||
|
/* publish actuator controls */
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
|
||||||
|
actuators.control[i] = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
||||||
|
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||||
|
|
||||||
|
/* subscribe */
|
||||||
|
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||||
|
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||||
|
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||||
|
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||||
|
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
|
|
||||||
|
/* Setup of loop */
|
||||||
|
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
float speed_body[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
|
||||||
|
|
||||||
|
while (!thread_should_exit) {
|
||||||
|
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||||
|
poll(&fds, 1, 500);
|
||||||
|
|
||||||
|
/* Check if there is a new position measurement or attitude setpoint */
|
||||||
|
bool pos_updated;
|
||||||
|
orb_check(global_pos_sub, &pos_updated);
|
||||||
|
bool att_sp_updated;
|
||||||
|
orb_check(att_sp_sub, &att_sp_updated);
|
||||||
|
|
||||||
|
/* get a local copy of attitude */
|
||||||
|
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||||
|
|
||||||
|
if (att_sp_updated)
|
||||||
|
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
|
||||||
|
|
||||||
|
if (pos_updated) {
|
||||||
|
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||||
|
|
||||||
|
if (att.R_valid) {
|
||||||
|
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
|
||||||
|
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
|
||||||
|
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
speed_body[0] = 0;
|
||||||
|
speed_body[1] = 0;
|
||||||
|
speed_body[2] = 0;
|
||||||
|
|
||||||
|
printf("FW ATT CONTROL: Did not get a valid R\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* welcome user */
|
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
|
||||||
printf("[fixedwing att_control] started\n");
|
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||||
|
|
||||||
/* declare and safely initialize all structs */
|
gyro[0] = att.rollspeed;
|
||||||
struct vehicle_attitude_s att;
|
gyro[1] = att.pitchspeed;
|
||||||
memset(&att, 0, sizeof(att));
|
gyro[2] = att.yawspeed;
|
||||||
struct vehicle_attitude_setpoint_s att_sp;
|
|
||||||
memset(&att_sp, 0, sizeof(att_sp));
|
|
||||||
struct vehicle_rates_setpoint_s rates_sp;
|
|
||||||
memset(&rates_sp, 0, sizeof(rates_sp));
|
|
||||||
struct manual_control_setpoint_s manual_sp;
|
|
||||||
memset(&manual_sp, 0, sizeof(manual_sp));
|
|
||||||
struct vehicle_status_s vstatus;
|
|
||||||
memset(&vstatus, 0, sizeof(vstatus));
|
|
||||||
|
|
||||||
// static struct debug_key_value_s debug_output;
|
/* control */
|
||||||
// memset(&debug_output, 0, sizeof(debug_output));
|
|
||||||
|
|
||||||
/* output structs */
|
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
|
||||||
struct actuator_controls_s actuators;
|
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||||
memset(&actuators, 0, sizeof(actuators));
|
/* attitude control */
|
||||||
|
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||||
|
|
||||||
|
/* angular rate control */
|
||||||
|
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
||||||
|
|
||||||
/* publish actuator controls */
|
/* pass through throttle */
|
||||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
|
actuators.control[3] = att_sp.thrust;
|
||||||
actuators.control[i] = 0.0f;
|
|
||||||
}
|
|
||||||
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
|
||||||
// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
|
|
||||||
// debug_output.key[0] = '1';
|
|
||||||
|
|
||||||
|
/* set flaps to zero */
|
||||||
|
actuators.control[4] = 0.0f;
|
||||||
|
|
||||||
/* subscribe */
|
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
||||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||||
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
|
||||||
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
|
||||||
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
|
||||||
|
|
||||||
/* Setup of loop */
|
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||||
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
if (vstatus.rc_signal_lost) {
|
||||||
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
|
|
||||||
|
|
||||||
while(!thread_should_exit)
|
/* put plane into loiter */
|
||||||
{
|
att_sp.roll_body = 0.3f;
|
||||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
att_sp.pitch_body = 0.0f;
|
||||||
poll(&fds, 1, 500);
|
|
||||||
|
|
||||||
/* get a local copy of attitude */
|
/* limit throttle to 60 % of last value if sane */
|
||||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
if (isfinite(manual_sp.throttle) &&
|
||||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
|
(manual_sp.throttle >= 0.0f) &&
|
||||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
|
(manual_sp.throttle <= 1.0f)) {
|
||||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
att_sp.thrust = 0.6f * manual_sp.throttle;
|
||||||
|
|
||||||
gyro[0] = att.rollspeed;
|
} else {
|
||||||
gyro[1] = att.pitchspeed;
|
att_sp.thrust = 0.0f;
|
||||||
gyro[2] = att.yawspeed;
|
}
|
||||||
|
|
||||||
/* Control */
|
att_sp.yaw_body = 0;
|
||||||
|
|
||||||
if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
|
// XXX disable yaw control, loiter
|
||||||
/* Attitude Control */
|
|
||||||
fixedwing_att_control_attitude(&att_sp,
|
|
||||||
&att,
|
|
||||||
&rates_sp);
|
|
||||||
|
|
||||||
/* Attitude Rate Control */
|
} else {
|
||||||
|
|
||||||
|
att_sp.roll_body = manual_sp.roll;
|
||||||
|
att_sp.pitch_body = manual_sp.pitch;
|
||||||
|
att_sp.yaw_body = 0;
|
||||||
|
att_sp.thrust = manual_sp.throttle;
|
||||||
|
}
|
||||||
|
|
||||||
|
att_sp.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
/* attitude control */
|
||||||
|
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
|
||||||
|
|
||||||
|
/* angular rate control */
|
||||||
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
|
||||||
|
|
||||||
//REMOVEME XXX
|
/* pass through throttle */
|
||||||
actuators.control[3] = 0.7f;
|
actuators.control[3] = att_sp.thrust;
|
||||||
|
|
||||||
// debug_output.value = rates_sp.pitch;
|
/* pass through flaps */
|
||||||
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
|
if (isfinite(manual_sp.flaps)) {
|
||||||
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
|
actuators.control[4] = manual_sp.flaps;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
actuators.control[4] = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||||
/* directly pass through values */
|
/* directly pass through values */
|
||||||
actuators.control[0] = manual_sp.roll;
|
actuators.control[0] = manual_sp.roll;
|
||||||
/* positive pitch means negative actuator -> pull up */
|
/* positive pitch means negative actuator -> pull up */
|
||||||
actuators.control[1] = -manual_sp.pitch;
|
actuators.control[1] = manual_sp.pitch;
|
||||||
actuators.control[2] = manual_sp.yaw;
|
actuators.control[2] = manual_sp.yaw;
|
||||||
actuators.control[3] = manual_sp.throttle;
|
actuators.control[3] = manual_sp.throttle;
|
||||||
}
|
|
||||||
|
|
||||||
/* publish output */
|
if (isfinite(manual_sp.flaps)) {
|
||||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
actuators.control[4] = manual_sp.flaps;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
actuators.control[4] = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
|
/* publish rates */
|
||||||
thread_running = false;
|
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
|
||||||
|
|
||||||
/* kill all outputs */
|
/* sanity check and publish actuator outputs */
|
||||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
if (isfinite(actuators.control[0]) &&
|
||||||
actuators.control[i] = 0.0f;
|
isfinite(actuators.control[1]) &&
|
||||||
|
isfinite(actuators.control[2]) &&
|
||||||
|
isfinite(actuators.control[3])) {
|
||||||
|
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
|
||||||
|
thread_running = false;
|
||||||
|
|
||||||
|
/* kill all outputs */
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||||
|
actuators.control[i] = 0.0f;
|
||||||
|
|
||||||
|
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
close(att_sub);
|
close(att_sub);
|
||||||
close(actuator_pub);
|
close(actuator_pub);
|
||||||
|
close(rates_pub);
|
||||||
|
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
exit(0);
|
exit(0);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -239,6 +308,7 @@ usage(const char *reason)
|
||||||
{
|
{
|
||||||
if (reason)
|
if (reason)
|
||||||
fprintf(stderr, "%s\n", reason);
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
|
||||||
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
|
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
@ -268,7 +338,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
|
||||||
deamon_task = task_spawn("fixedwing_att_control",
|
deamon_task = task_spawn("fixedwing_att_control",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_MAX - 20,
|
SCHED_PRIORITY_MAX - 20,
|
||||||
4096,
|
2048,
|
||||||
fixedwing_att_control_thread_main,
|
fixedwing_att_control_thread_main,
|
||||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
thread_running = true;
|
thread_running = true;
|
||||||
|
@ -283,9 +353,11 @@ int fixedwing_att_control_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (thread_running) {
|
if (thread_running) {
|
||||||
printf("\tfixedwing_att_control is running\n");
|
printf("\tfixedwing_att_control is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("\tfixedwing_att_control not started\n");
|
printf("\tfixedwing_att_control not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -34,6 +34,8 @@
|
||||||
/**
|
/**
|
||||||
* @file fixedwing_att_control_rate.c
|
* @file fixedwing_att_control_rate.c
|
||||||
* Implementation of a fixed wing attitude controller.
|
* Implementation of a fixed wing attitude controller.
|
||||||
|
*
|
||||||
|
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
*/
|
*/
|
||||||
#include <fixedwing_att_control_rate.h>
|
#include <fixedwing_att_control_rate.h>
|
||||||
|
|
||||||
|
@ -59,9 +61,33 @@
|
||||||
#include <systemlib/geo/geo.h>
|
#include <systemlib/geo/geo.h>
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Controller parameters, accessible via MAVLink
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
// Roll control parameters
|
||||||
|
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
|
||||||
|
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
|
||||||
|
|
||||||
|
//Pitch control parameters
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
|
||||||
|
|
||||||
|
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
|
||||||
|
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
|
||||||
|
|
||||||
|
/* feedforward compensation */
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */
|
||||||
|
|
||||||
struct fw_rate_control_params {
|
struct fw_rate_control_params {
|
||||||
float rollrate_p;
|
float rollrate_p;
|
||||||
|
@ -73,7 +99,7 @@ struct fw_rate_control_params {
|
||||||
float yawrate_p;
|
float yawrate_p;
|
||||||
float yawrate_i;
|
float yawrate_i;
|
||||||
float yawrate_awu;
|
float yawrate_awu;
|
||||||
|
float pitch_thr_ff;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct fw_rate_control_param_handles {
|
struct fw_rate_control_param_handles {
|
||||||
|
@ -86,7 +112,7 @@ struct fw_rate_control_param_handles {
|
||||||
param_t yawrate_p;
|
param_t yawrate_p;
|
||||||
param_t yawrate_i;
|
param_t yawrate_i;
|
||||||
param_t yawrate_awu;
|
param_t yawrate_awu;
|
||||||
|
param_t pitch_thr_ff;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -98,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
|
||||||
static int parameters_init(struct fw_rate_control_param_handles *h)
|
static int parameters_init(struct fw_rate_control_param_handles *h)
|
||||||
{
|
{
|
||||||
/* PID parameters */
|
/* PID parameters */
|
||||||
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
|
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
|
||||||
h->rollrate_i = param_find("FW_ROLLR_I");
|
h->rollrate_i = param_find("FW_ROLLR_I");
|
||||||
h->rollrate_awu = param_find("FW_ROLLR_AWU");
|
h->rollrate_awu = param_find("FW_ROLLR_AWU");
|
||||||
|
|
||||||
h->pitchrate_p = param_find("FW_PITCHR_P");
|
h->pitchrate_p = param_find("FW_PITCHR_P");
|
||||||
h->pitchrate_i = param_find("FW_PITCHR_I");
|
h->pitchrate_i = param_find("FW_PITCHR_I");
|
||||||
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
|
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
|
||||||
|
|
||||||
h->yawrate_p = param_find("FW_YAWR_P");
|
h->yawrate_p = param_find("FW_YAWR_P");
|
||||||
h->yawrate_i = param_find("FW_YAWR_I");
|
h->yawrate_i = param_find("FW_YAWR_I");
|
||||||
h->yawrate_awu = param_find("FW_YAWR_AWU");
|
h->yawrate_awu = param_find("FW_YAWR_AWU");
|
||||||
|
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
@ -124,14 +151,14 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
|
||||||
param_get(h->yawrate_p, &(p->yawrate_p));
|
param_get(h->yawrate_p, &(p->yawrate_p));
|
||||||
param_get(h->yawrate_i, &(p->yawrate_i));
|
param_get(h->yawrate_i, &(p->yawrate_i));
|
||||||
param_get(h->yawrate_awu, &(p->yawrate_awu));
|
param_get(h->yawrate_awu, &(p->yawrate_awu));
|
||||||
|
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||||
const float rates[],
|
const float rates[],
|
||||||
struct actuator_controls_s *actuators)
|
struct actuator_controls_s *actuators)
|
||||||
{
|
{
|
||||||
static int counter = 0;
|
static int counter = 0;
|
||||||
static bool initialized = false;
|
static bool initialized = false;
|
||||||
|
@ -147,8 +174,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||||
last_run = hrt_absolute_time();
|
last_run = hrt_absolute_time();
|
||||||
|
|
||||||
if(!initialized)
|
if (!initialized) {
|
||||||
{
|
|
||||||
parameters_init(&h);
|
parameters_init(&h);
|
||||||
parameters_update(&h, &p);
|
parameters_update(&h, &p);
|
||||||
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
|
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
|
||||||
|
@ -167,12 +193,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/* Roll Rate (PI) */
|
/* roll rate (PI) */
|
||||||
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
|
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
|
||||||
|
/* pitch rate (PI) */
|
||||||
|
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
|
||||||
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer...
|
/* set pitch minus feedforward throttle compensation (nose pitches up from throttle */
|
||||||
actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now
|
actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust;
|
||||||
|
/* yaw rate (PI) */
|
||||||
|
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
|
||||||
|
|
||||||
counter++;
|
counter++;
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,7 @@
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
|
||||||
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||||
const float rates[],
|
const float rates[],
|
||||||
struct actuator_controls_s *actuators);
|
struct actuator_controls_s *actuators);
|
||||||
|
|
||||||
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
|
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
|
||||||
|
|
|
@ -1,566 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
|
||||||
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
|
|
||||||
* Modifications: Doug Weibel <douglas.weibel@colorado.edu>
|
|
||||||
*
|
|
||||||
* 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 fixedwing_control.c
|
|
||||||
* Implementation of a fixed wing attitude and position controller.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <nuttx/config.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include <poll.h>
|
|
||||||
#include <time.h>
|
|
||||||
#include <drivers/drv_hrt.h>
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
#include <uORB/uORB.h>
|
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
|
||||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
|
||||||
#include <uORB/topics/vehicle_status.h>
|
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
|
||||||
#include <systemlib/param/param.h>
|
|
||||||
#include <systemlib/pid/pid.h>
|
|
||||||
#include <systemlib/geo/geo.h>
|
|
||||||
#include <systemlib/systemlib.h>
|
|
||||||
#include <uORB/topics/debug_key_value.h>
|
|
||||||
|
|
||||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
|
||||||
static bool thread_running = false; /**< Deamon status flag */
|
|
||||||
static int deamon_task; /**< Handle of deamon task / thread */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Deamon management function.
|
|
||||||
*/
|
|
||||||
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Mainloop of deamon.
|
|
||||||
*/
|
|
||||||
int fixedwing_control_thread_main(int argc, char *argv[]);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Print the correct usage.
|
|
||||||
*/
|
|
||||||
static void usage(const char *reason);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Controller parameters, accessible via MAVLink
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
// Roll control parameters
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
|
|
||||||
// Need to add functionality to suppress integrator windup while on the ground
|
|
||||||
// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
|
|
||||||
|
|
||||||
//Pitch control parameters
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
|
|
||||||
// Need to add functionality to suppress integrator windup while on the ground
|
|
||||||
// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
|
|
||||||
|
|
||||||
struct fw_att_control_params {
|
|
||||||
float rollrate_p;
|
|
||||||
float rollrate_i;
|
|
||||||
float rollrate_awu;
|
|
||||||
float rollrate_lim;
|
|
||||||
float roll_p;
|
|
||||||
float roll_lim;
|
|
||||||
float pitchrate_p;
|
|
||||||
float pitchrate_i;
|
|
||||||
float pitchrate_awu;
|
|
||||||
float pitchrate_lim;
|
|
||||||
float pitch_p;
|
|
||||||
float pitch_lim;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct fw_att_control_param_handles {
|
|
||||||
param_t rollrate_p;
|
|
||||||
param_t rollrate_i;
|
|
||||||
param_t rollrate_awu;
|
|
||||||
param_t rollrate_lim;
|
|
||||||
param_t roll_p;
|
|
||||||
param_t roll_lim;
|
|
||||||
param_t pitchrate_p;
|
|
||||||
param_t pitchrate_i;
|
|
||||||
param_t pitchrate_awu;
|
|
||||||
param_t pitchrate_lim;
|
|
||||||
param_t pitch_p;
|
|
||||||
param_t pitch_lim;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
// TO_DO - Navigation control will be moved to a separate app
|
|
||||||
// Attitude control will just handle the inner angle and rate loops
|
|
||||||
// to control pitch and roll, and turn coordination via rudder and
|
|
||||||
// possibly throttle compensation for battery voltage sag.
|
|
||||||
|
|
||||||
PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
|
|
||||||
PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f);
|
|
||||||
|
|
||||||
struct fw_pos_control_params {
|
|
||||||
float heading_p;
|
|
||||||
float heading_lim;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct fw_pos_control_param_handles {
|
|
||||||
param_t heading_p;
|
|
||||||
param_t heading_lim;
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Initialize all parameter handles and values
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
static int att_parameters_init(struct fw_att_control_param_handles *h);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Update all parameters
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Initialize all parameter handles and values
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
static int pos_parameters_init(struct fw_pos_control_param_handles *h);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Update all parameters
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* The fixed wing control main thread.
|
|
||||||
*
|
|
||||||
* The main loop executes continously and calculates the control
|
|
||||||
* response.
|
|
||||||
*
|
|
||||||
* @param argc number of arguments
|
|
||||||
* @param argv argument array
|
|
||||||
*
|
|
||||||
* @return 0
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
int fixedwing_control_thread_main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
/* read arguments */
|
|
||||||
bool verbose = false;
|
|
||||||
|
|
||||||
for (int i = 1; i < argc; i++) {
|
|
||||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
|
||||||
verbose = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* welcome user */
|
|
||||||
printf("[fixedwing control] started\n");
|
|
||||||
|
|
||||||
/* output structs */
|
|
||||||
struct actuator_controls_s actuators;
|
|
||||||
struct vehicle_attitude_setpoint_s att_sp;
|
|
||||||
memset(&att_sp, 0, sizeof(att_sp));
|
|
||||||
|
|
||||||
/* publish actuator controls */
|
|
||||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
|
||||||
actuators.control[i] = 0.0f;
|
|
||||||
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
|
||||||
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
|
||||||
|
|
||||||
/* Subscribe to global position, attitude and rc */
|
|
||||||
/* declare and safely initialize all structs */
|
|
||||||
struct vehicle_status_s state;
|
|
||||||
memset(&state, 0, sizeof(state));
|
|
||||||
struct vehicle_attitude_s att;
|
|
||||||
memset(&att_sp, 0, sizeof(att_sp));
|
|
||||||
struct manual_control_setpoint_s manual;
|
|
||||||
memset(&manual, 0, sizeof(manual));
|
|
||||||
|
|
||||||
/* subscribe to attitude, motor setpoints and system state */
|
|
||||||
struct vehicle_global_position_s global_pos;
|
|
||||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
|
||||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
|
||||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
|
||||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
|
||||||
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
|
||||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
|
||||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
|
||||||
|
|
||||||
/* Mainloop setup */
|
|
||||||
unsigned int loopcounter = 0;
|
|
||||||
|
|
||||||
uint64_t last_run = 0;
|
|
||||||
uint64_t last_run_pos = 0;
|
|
||||||
|
|
||||||
bool global_sp_updated_set_once = false;
|
|
||||||
|
|
||||||
struct fw_att_control_params p;
|
|
||||||
struct fw_att_control_param_handles h;
|
|
||||||
|
|
||||||
struct fw_pos_control_params ppos;
|
|
||||||
struct fw_pos_control_param_handles hpos;
|
|
||||||
|
|
||||||
/* initialize the pid controllers */
|
|
||||||
att_parameters_init(&h);
|
|
||||||
att_parameters_update(&h, &p);
|
|
||||||
|
|
||||||
pos_parameters_init(&hpos);
|
|
||||||
pos_parameters_update(&hpos, &ppos);
|
|
||||||
|
|
||||||
// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere
|
|
||||||
PID_t roll_rate_controller;
|
|
||||||
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu,
|
|
||||||
p.rollrate_lim,PID_MODE_DERIVATIV_NONE);
|
|
||||||
PID_t roll_angle_controller;
|
|
||||||
pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f,
|
|
||||||
p.roll_lim,PID_MODE_DERIVATIV_NONE);
|
|
||||||
|
|
||||||
PID_t pitch_rate_controller;
|
|
||||||
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu,
|
|
||||||
p.pitchrate_lim,PID_MODE_DERIVATIV_NONE);
|
|
||||||
PID_t pitch_angle_controller;
|
|
||||||
pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f,
|
|
||||||
p.pitch_lim,PID_MODE_DERIVATIV_NONE);
|
|
||||||
|
|
||||||
PID_t heading_controller;
|
|
||||||
pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
|
|
||||||
100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit
|
|
||||||
|
|
||||||
// XXX remove in production
|
|
||||||
/* advertise debug value */
|
|
||||||
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
|
||||||
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
|
|
||||||
|
|
||||||
// This is the top of the main loop
|
|
||||||
while(!thread_should_exit) {
|
|
||||||
|
|
||||||
struct pollfd fds[1] = {
|
|
||||||
{ .fd = att_sub, .events = POLLIN },
|
|
||||||
};
|
|
||||||
int ret = poll(fds, 1, 1000);
|
|
||||||
|
|
||||||
if (ret < 0) {
|
|
||||||
/* XXX this is seriously bad - should be an emergency */
|
|
||||||
} else if (ret == 0) {
|
|
||||||
/* XXX this means no sensor data - should be critical or emergency */
|
|
||||||
printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n");
|
|
||||||
} else {
|
|
||||||
|
|
||||||
// FIXME SUBSCRIBE
|
|
||||||
if (loopcounter % 100 == 0) {
|
|
||||||
att_parameters_update(&h, &p);
|
|
||||||
pos_parameters_update(&hpos, &ppos);
|
|
||||||
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f,
|
|
||||||
p.rollrate_awu, p.rollrate_lim);
|
|
||||||
pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f,
|
|
||||||
0.0f, p.roll_lim);
|
|
||||||
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f,
|
|
||||||
p.pitchrate_awu, p.pitchrate_lim);
|
|
||||||
pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f,
|
|
||||||
0.0f, p.pitch_lim);
|
|
||||||
pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f);
|
|
||||||
//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n",
|
|
||||||
//p.rollrate_p, p.rollrate_i, p.rollrate_lim);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* if position updated, run position control loop */
|
|
||||||
bool pos_updated;
|
|
||||||
orb_check(global_pos_sub, &pos_updated);
|
|
||||||
bool global_sp_updated;
|
|
||||||
orb_check(global_setpoint_sub, &global_sp_updated);
|
|
||||||
if (global_sp_updated) {
|
|
||||||
global_sp_updated_set_once = true;
|
|
||||||
}
|
|
||||||
/* checking has to happen before the read, as the read clears the changed flag */
|
|
||||||
|
|
||||||
/* get a local copy of system state */
|
|
||||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
|
||||||
/* get a local copy of manual setpoint */
|
|
||||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
|
||||||
/* get a local copy of attitude */
|
|
||||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
|
||||||
/* get a local copy of attitude setpoint */
|
|
||||||
//orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
|
|
||||||
// XXX update to switch between external attitude reference and the
|
|
||||||
// attitude calculated here
|
|
||||||
|
|
||||||
char name[10];
|
|
||||||
|
|
||||||
if (pos_updated) {
|
|
||||||
|
|
||||||
/* get position */
|
|
||||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
|
||||||
|
|
||||||
if (global_sp_updated_set_once) {
|
|
||||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
|
||||||
|
|
||||||
|
|
||||||
/* calculate delta T */
|
|
||||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
|
||||||
last_run = hrt_absolute_time();
|
|
||||||
|
|
||||||
/* calculate bearing error */
|
|
||||||
float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d,
|
|
||||||
global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
|
|
||||||
|
|
||||||
/* shift error to prevent wrapping issues */
|
|
||||||
float bearing_error = target_bearing - att.yaw;
|
|
||||||
|
|
||||||
if (loopcounter % 2 == 0) {
|
|
||||||
sprintf(name, "hdng err1");
|
|
||||||
memcpy(dbg.key, name, sizeof(name));
|
|
||||||
dbg.value = bearing_error;
|
|
||||||
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (bearing_error < M_PI_F) {
|
|
||||||
bearing_error += 2.0f * M_PI_F;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (bearing_error > M_PI_F) {
|
|
||||||
bearing_error -= 2.0f * M_PI_F;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (loopcounter % 2 != 0) {
|
|
||||||
sprintf(name, "hdng err2");
|
|
||||||
memcpy(dbg.key, name, sizeof(name));
|
|
||||||
dbg.value = bearing_error;
|
|
||||||
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* calculate roll setpoint, do this artificially around zero */
|
|
||||||
att_sp.roll_body = pid_calculate(&heading_controller, bearing_error,
|
|
||||||
0.0f, att.yawspeed, deltaT);
|
|
||||||
|
|
||||||
/* limit roll angle output */
|
|
||||||
if (att_sp.roll_body > ppos.heading_lim) {
|
|
||||||
att_sp.roll_body = ppos.heading_lim;
|
|
||||||
heading_controller.saturated = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (att_sp.roll_body < -ppos.heading_lim) {
|
|
||||||
att_sp.roll_body = -ppos.heading_lim;
|
|
||||||
heading_controller.saturated = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
att_sp.pitch_body = 0.0f;
|
|
||||||
att_sp.yaw_body = 0.0f;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
/* no setpoint, maintain level flight */
|
|
||||||
att_sp.roll_body = 0.0f;
|
|
||||||
att_sp.pitch_body = 0.0f;
|
|
||||||
att_sp.yaw_body = 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
att_sp.thrust = 0.7f;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* calculate delta T */
|
|
||||||
const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f;
|
|
||||||
last_run_pos = hrt_absolute_time();
|
|
||||||
|
|
||||||
if (verbose && (loopcounter % 20 == 0)) {
|
|
||||||
printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
|
|
||||||
}
|
|
||||||
|
|
||||||
// actuator control[0] is aileron (or elevon roll control)
|
|
||||||
// Commanded roll rate from P controller on roll angle
|
|
||||||
float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body,
|
|
||||||
att.roll, 0.0f, deltaTpos);
|
|
||||||
// actuator control from PI controller on roll rate
|
|
||||||
actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command,
|
|
||||||
att.rollspeed, 0.0f, deltaTpos);
|
|
||||||
|
|
||||||
// actuator control[1] is elevator (or elevon pitch control)
|
|
||||||
// Commanded pitch rate from P controller on pitch angle
|
|
||||||
float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body,
|
|
||||||
att.pitch, 0.0f, deltaTpos);
|
|
||||||
// actuator control from PI controller on pitch rate
|
|
||||||
actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command,
|
|
||||||
att.pitchspeed, 0.0f, deltaTpos);
|
|
||||||
|
|
||||||
// actuator control[3] is throttle
|
|
||||||
actuators.control[3] = att_sp.thrust;
|
|
||||||
|
|
||||||
/* publish attitude setpoint (for MAVLink) */
|
|
||||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
|
||||||
|
|
||||||
/* publish actuator setpoints (for mixer) */
|
|
||||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
|
||||||
|
|
||||||
loopcounter++;
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("[fixedwing_control] exiting.\n");
|
|
||||||
thread_running = false;
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void
|
|
||||||
usage(const char *reason)
|
|
||||||
{
|
|
||||||
if (reason)
|
|
||||||
fprintf(stderr, "%s\n", reason);
|
|
||||||
fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n");
|
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* The 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_create().
|
|
||||||
*/
|
|
||||||
int fixedwing_control_main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
if (argc < 1)
|
|
||||||
usage("missing command");
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "start")) {
|
|
||||||
|
|
||||||
if (thread_running) {
|
|
||||||
printf("fixedwing_control already running\n");
|
|
||||||
/* this is not an error */
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
thread_should_exit = false;
|
|
||||||
deamon_task = task_spawn("fixedwing_control",
|
|
||||||
SCHED_DEFAULT,
|
|
||||||
SCHED_PRIORITY_MAX - 20,
|
|
||||||
4096,
|
|
||||||
fixedwing_control_thread_main,
|
|
||||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
|
||||||
thread_running = true;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "stop")) {
|
|
||||||
thread_should_exit = true;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcmp(argv[1], "status")) {
|
|
||||||
if (thread_running) {
|
|
||||||
printf("\tfixedwing_control is running\n");
|
|
||||||
} else {
|
|
||||||
printf("\tfixedwing_control not started\n");
|
|
||||||
}
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
usage("unrecognized command");
|
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
static int att_parameters_init(struct fw_att_control_param_handles *h)
|
|
||||||
{
|
|
||||||
/* PID parameters */
|
|
||||||
|
|
||||||
h->rollrate_p = param_find("FW_ROLLRATE_P");
|
|
||||||
h->rollrate_i = param_find("FW_ROLLRATE_I");
|
|
||||||
h->rollrate_awu = param_find("FW_ROLLRATE_AWU");
|
|
||||||
h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
|
|
||||||
h->roll_p = param_find("FW_ROLL_P");
|
|
||||||
h->roll_lim = param_find("FW_ROLL_LIM");
|
|
||||||
h->pitchrate_p = param_find("FW_PITCHRATE_P");
|
|
||||||
h->pitchrate_i = param_find("FW_PITCHRATE_I");
|
|
||||||
h->pitchrate_awu = param_find("FW_PITCHRATE_AWU");
|
|
||||||
h->pitchrate_lim = param_find("FW_PITCHRATE_LIM");
|
|
||||||
h->pitch_p = param_find("FW_PITCH_P");
|
|
||||||
h->pitch_lim = param_find("FW_PITCH_LIM");
|
|
||||||
|
|
||||||
return OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p)
|
|
||||||
{
|
|
||||||
param_get(h->rollrate_p, &(p->rollrate_p));
|
|
||||||
param_get(h->rollrate_i, &(p->rollrate_i));
|
|
||||||
param_get(h->rollrate_awu, &(p->rollrate_awu));
|
|
||||||
param_get(h->rollrate_lim, &(p->rollrate_lim));
|
|
||||||
param_get(h->roll_p, &(p->roll_p));
|
|
||||||
param_get(h->roll_lim, &(p->roll_lim));
|
|
||||||
param_get(h->pitchrate_p, &(p->pitchrate_p));
|
|
||||||
param_get(h->pitchrate_i, &(p->pitchrate_i));
|
|
||||||
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
|
|
||||||
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
|
|
||||||
param_get(h->pitch_p, &(p->pitch_p));
|
|
||||||
param_get(h->pitch_lim, &(p->pitch_lim));
|
|
||||||
|
|
||||||
return OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int pos_parameters_init(struct fw_pos_control_param_handles *h)
|
|
||||||
{
|
|
||||||
/* PID parameters */
|
|
||||||
h->heading_p = param_find("FW_HEADING_P");
|
|
||||||
h->heading_lim = param_find("FW_HEADING_LIM");
|
|
||||||
|
|
||||||
return OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
|
|
||||||
{
|
|
||||||
param_get(h->heading_p, &(p->heading_p));
|
|
||||||
param_get(h->heading_lim, &(p->heading_lim));
|
|
||||||
|
|
||||||
return OK;
|
|
||||||
}
|
|
|
@ -57,24 +57,31 @@
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <systemlib/param/param.h>
|
#include <systemlib/param/param.h>
|
||||||
#include <systemlib/pid/pid.h>
|
#include <systemlib/pid/pid.h>
|
||||||
#include <systemlib/geo/geo.h>
|
#include <systemlib/geo/geo.h>
|
||||||
|
#include <systemlib/perf_counter.h>
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Controller parameters, accessible via MAVLink
|
* Controller parameters, accessible via MAVLink
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
|
PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
|
||||||
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
|
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
|
||||||
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
|
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
|
||||||
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
|
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
|
||||||
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
|
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
|
||||||
|
|
||||||
struct fw_pos_control_params {
|
struct fw_pos_control_params {
|
||||||
float heading_p;
|
float heading_p;
|
||||||
|
float headingr_p;
|
||||||
|
float headingr_i;
|
||||||
|
float headingr_lim;
|
||||||
float xtrack_p;
|
float xtrack_p;
|
||||||
float altitude_p;
|
float altitude_p;
|
||||||
float roll_lim;
|
float roll_lim;
|
||||||
|
@ -83,11 +90,13 @@ struct fw_pos_control_params {
|
||||||
|
|
||||||
struct fw_pos_control_param_handles {
|
struct fw_pos_control_param_handles {
|
||||||
param_t heading_p;
|
param_t heading_p;
|
||||||
|
param_t headingr_p;
|
||||||
|
param_t headingr_i;
|
||||||
|
param_t headingr_lim;
|
||||||
param_t xtrack_p;
|
param_t xtrack_p;
|
||||||
param_t altitude_p;
|
param_t altitude_p;
|
||||||
param_t roll_lim;
|
param_t roll_lim;
|
||||||
param_t pitch_lim;
|
param_t pitch_lim;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -99,8 +108,8 @@ struct planned_path_segments_s {
|
||||||
double end_lon;
|
double end_lon;
|
||||||
float radius; // Radius of arc
|
float radius; // Radius of arc
|
||||||
float arc_start_bearing; // Bearing from center to start of arc
|
float arc_start_bearing; // Bearing from center to start of arc
|
||||||
float arc_sweep; // Angle (radians) swept out by arc around center.
|
float arc_sweep; // Angle (radians) swept out by arc around center.
|
||||||
// Positive for clockwise, negative for counter-clockwise
|
// Positive for clockwise, negative for counter-clockwise
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -136,12 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */
|
||||||
static int parameters_init(struct fw_pos_control_param_handles *h)
|
static int parameters_init(struct fw_pos_control_param_handles *h)
|
||||||
{
|
{
|
||||||
/* PID parameters */
|
/* PID parameters */
|
||||||
h->heading_p = param_find("FW_HEADING_P");
|
h->heading_p = param_find("FW_HEAD_P");
|
||||||
h->xtrack_p = param_find("FW_XTRACK_P");
|
h->headingr_p = param_find("FW_HEADR_P");
|
||||||
h->altitude_p = param_find("FW_ALT_P");
|
h->headingr_i = param_find("FW_HEADR_I");
|
||||||
h->roll_lim = param_find("FW_ROLL_LIM");
|
h->headingr_lim = param_find("FW_HEADR_LIM");
|
||||||
h->pitch_lim = param_find("FW_PITCH_LIM");
|
h->xtrack_p = param_find("FW_XTRACK_P");
|
||||||
|
h->altitude_p = param_find("FW_ALT_P");
|
||||||
|
h->roll_lim = param_find("FW_ROLL_LIM");
|
||||||
|
h->pitch_lim = param_find("FW_PITCH_LIM");
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
@ -149,6 +160,9 @@ static int parameters_init(struct fw_pos_control_param_handles *h)
|
||||||
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
|
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
|
||||||
{
|
{
|
||||||
param_get(h->heading_p, &(p->heading_p));
|
param_get(h->heading_p, &(p->heading_p));
|
||||||
|
param_get(h->headingr_p, &(p->headingr_p));
|
||||||
|
param_get(h->headingr_i, &(p->headingr_i));
|
||||||
|
param_get(h->headingr_lim, &(p->headingr_lim));
|
||||||
param_get(h->xtrack_p, &(p->xtrack_p));
|
param_get(h->xtrack_p, &(p->xtrack_p));
|
||||||
param_get(h->altitude_p, &(p->altitude_p));
|
param_get(h->altitude_p, &(p->altitude_p));
|
||||||
param_get(h->roll_lim, &(p->roll_lim));
|
param_get(h->roll_lim, &(p->roll_lim));
|
||||||
|
@ -162,172 +176,241 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
|
||||||
int fixedwing_pos_control_thread_main(int argc, char *argv[])
|
int fixedwing_pos_control_thread_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
/* read arguments */
|
/* read arguments */
|
||||||
bool verbose = false;
|
bool verbose = false;
|
||||||
|
|
||||||
for (int i = 1; i < argc; i++) {
|
for (int i = 1; i < argc; i++) {
|
||||||
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
|
||||||
verbose = true;
|
verbose = true;
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/* welcome user */
|
/* welcome user */
|
||||||
printf("[fixedwing att_control] started\n");
|
printf("[fixedwing pos control] started\n");
|
||||||
|
|
||||||
/* declare and safely initialize all structs */
|
/* declare and safely initialize all structs */
|
||||||
struct vehicle_global_position_s global_pos;
|
struct vehicle_global_position_s global_pos;
|
||||||
memset(&global_pos, 0, sizeof(global_pos));
|
memset(&global_pos, 0, sizeof(global_pos));
|
||||||
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
|
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
|
||||||
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
|
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
|
||||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
struct vehicle_global_position_setpoint_s global_setpoint;
|
||||||
memset(&global_setpoint, 0, sizeof(global_setpoint));
|
memset(&global_setpoint, 0, sizeof(global_setpoint));
|
||||||
struct vehicle_attitude_s att;
|
struct vehicle_attitude_s att;
|
||||||
memset(&att, 0, sizeof(att));
|
memset(&att, 0, sizeof(att));
|
||||||
struct crosstrack_error_s xtrack_err;
|
struct crosstrack_error_s xtrack_err;
|
||||||
memset(&xtrack_err, 0, sizeof(xtrack_err));
|
memset(&xtrack_err, 0, sizeof(xtrack_err));
|
||||||
|
struct parameter_update_s param_update;
|
||||||
/* output structs */
|
memset(¶m_update, 0, sizeof(param_update));
|
||||||
struct vehicle_attitude_setpoint_s attitude_setpoint;
|
|
||||||
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
|
|
||||||
|
|
||||||
/* publish attitude setpoint */
|
/* output structs */
|
||||||
attitude_setpoint.roll_body = 0.0f;
|
struct vehicle_attitude_setpoint_s attitude_setpoint;
|
||||||
attitude_setpoint.pitch_body = 0.0f;
|
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
|
||||||
attitude_setpoint.yaw_body = 0.0f;
|
|
||||||
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
|
|
||||||
|
|
||||||
/* subscribe */
|
/* publish attitude setpoint */
|
||||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
attitude_setpoint.roll_body = 0.0f;
|
||||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
attitude_setpoint.pitch_body = 0.0f;
|
||||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
attitude_setpoint.yaw_body = 0.0f;
|
||||||
|
attitude_setpoint.thrust = 0.0f;
|
||||||
|
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
|
||||||
|
|
||||||
/* Setup of loop */
|
/* subscribe */
|
||||||
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
|
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||||
bool global_sp_updated_set_once = false;
|
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||||
|
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||||
|
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||||
|
|
||||||
float psi_track = 0.0f;
|
/* Setup of loop */
|
||||||
|
struct pollfd fds[2] = {
|
||||||
|
{ .fd = param_sub, .events = POLLIN },
|
||||||
|
{ .fd = att_sub, .events = POLLIN }
|
||||||
|
};
|
||||||
|
bool global_sp_updated_set_once = false;
|
||||||
|
|
||||||
while(!thread_should_exit)
|
float psi_track = 0.0f;
|
||||||
{
|
|
||||||
/* wait for a sensor update, check for exit condition every 500 ms */
|
|
||||||
poll(&fds, 1, 500);
|
|
||||||
|
|
||||||
static int counter = 0;
|
int counter = 0;
|
||||||
static bool initialized = false;
|
|
||||||
|
|
||||||
static struct fw_pos_control_params p;
|
struct fw_pos_control_params p;
|
||||||
static struct fw_pos_control_param_handles h;
|
struct fw_pos_control_param_handles h;
|
||||||
|
|
||||||
PID_t heading_controller;
|
PID_t heading_controller;
|
||||||
PID_t altitude_controller;
|
PID_t heading_rate_controller;
|
||||||
|
PID_t offtrack_controller;
|
||||||
|
PID_t altitude_controller;
|
||||||
|
|
||||||
if(!initialized)
|
parameters_init(&h);
|
||||||
{
|
parameters_update(&h, &p);
|
||||||
parameters_init(&h);
|
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
|
||||||
parameters_update(&h, &p);
|
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
|
||||||
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
|
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
|
||||||
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
|
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
|
||||||
initialized = true;
|
|
||||||
}
|
/* error and performance monitoring */
|
||||||
|
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
|
||||||
|
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
|
||||||
|
|
||||||
|
while (!thread_should_exit) {
|
||||||
|
/* wait for a sensor update, check for exit condition every 500 ms */
|
||||||
|
int ret = poll(fds, 2, 500);
|
||||||
|
|
||||||
|
if (ret < 0) {
|
||||||
|
/* poll error, count it in perf */
|
||||||
|
perf_count(fw_err_perf);
|
||||||
|
|
||||||
|
} else if (ret == 0) {
|
||||||
|
/* no return value, ignore */
|
||||||
|
} else {
|
||||||
|
|
||||||
|
/* only update parameters if they changed */
|
||||||
|
if (fds[0].revents & POLLIN) {
|
||||||
|
/* read from param to clear updated flag */
|
||||||
|
struct parameter_update_s update;
|
||||||
|
orb_copy(ORB_ID(parameter_update), param_sub, &update);
|
||||||
|
|
||||||
/* load new parameters with lower rate */
|
|
||||||
if (counter % 100 == 0) {
|
|
||||||
/* update parameters from storage */
|
/* update parameters from storage */
|
||||||
parameters_update(&h, &p);
|
parameters_update(&h, &p);
|
||||||
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
|
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
|
||||||
|
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
|
||||||
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
|
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
|
||||||
|
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Check if there is a new position or setpoint */
|
/* only run controller if attitude changed */
|
||||||
bool pos_updated;
|
if (fds[1].revents & POLLIN) {
|
||||||
orb_check(global_pos_sub, &pos_updated);
|
|
||||||
bool global_sp_updated;
|
|
||||||
orb_check(global_setpoint_sub, &global_sp_updated);
|
|
||||||
|
|
||||||
/* Load local copies */
|
|
||||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
|
||||||
if(pos_updated)
|
|
||||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
|
||||||
if (global_sp_updated)
|
|
||||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
|
||||||
|
|
||||||
if(global_sp_updated) {
|
|
||||||
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
|
|
||||||
global_sp_updated_set_once = true;
|
|
||||||
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
|
||||||
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
|
||||||
printf("psi_track: %0.4f\n", (double)psi_track);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Control */
|
|
||||||
|
|
||||||
|
|
||||||
/* Simple Horizontal Control */
|
static uint64_t last_run = 0;
|
||||||
if(global_sp_updated_set_once)
|
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||||
{
|
last_run = hrt_absolute_time();
|
||||||
// if (counter % 100 == 0)
|
|
||||||
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
|
|
||||||
|
|
||||||
/* calculate crosstrack error */
|
|
||||||
// Only the case of a straight line track following handled so far
|
|
||||||
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
|
||||||
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
|
|
||||||
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
|
||||||
|
|
||||||
if(!(distance_res != OK || xtrack_err.past_end)) {
|
/* check if there is a new position or setpoint */
|
||||||
|
bool pos_updated;
|
||||||
|
orb_check(global_pos_sub, &pos_updated);
|
||||||
|
bool global_sp_updated;
|
||||||
|
orb_check(global_setpoint_sub, &global_sp_updated);
|
||||||
|
|
||||||
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
|
/* load local copies */
|
||||||
|
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||||
|
|
||||||
if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
|
if (pos_updated) {
|
||||||
delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
|
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||||
|
|
||||||
if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
|
|
||||||
delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
|
|
||||||
|
|
||||||
float psi_c = psi_track + delta_psi_c;
|
|
||||||
|
|
||||||
float psi_e = psi_c - att.yaw;
|
|
||||||
|
|
||||||
/* shift error to prevent wrapping issues */
|
|
||||||
psi_e = _wrap_pi(psi_e);
|
|
||||||
|
|
||||||
/* calculate roll setpoint, do this artificially around zero */
|
|
||||||
attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
|
|
||||||
|
|
||||||
// if (counter % 100 == 0)
|
|
||||||
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (counter % 100 == 0)
|
|
||||||
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (global_sp_updated) {
|
||||||
|
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
||||||
|
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
|
||||||
|
global_sp_updated_set_once = true;
|
||||||
|
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||||
|
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
||||||
|
|
||||||
|
printf("next wp direction: %0.4f\n", (double)psi_track);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Simple Horizontal Control */
|
||||||
|
if (global_sp_updated_set_once) {
|
||||||
|
// if (counter % 100 == 0)
|
||||||
|
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
|
||||||
|
|
||||||
|
/* calculate crosstrack error */
|
||||||
|
// Only the case of a straight line track following handled so far
|
||||||
|
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
|
||||||
|
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
|
||||||
|
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
|
||||||
|
|
||||||
|
// XXX what is xtrack_err.past_end?
|
||||||
|
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
|
||||||
|
|
||||||
|
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
|
||||||
|
|
||||||
|
float psi_c = psi_track + delta_psi_c;
|
||||||
|
float psi_e = psi_c - att.yaw;
|
||||||
|
|
||||||
|
/* wrap difference back onto -pi..pi range */
|
||||||
|
psi_e = _wrap_pi(psi_e);
|
||||||
|
|
||||||
|
if (verbose) {
|
||||||
|
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
|
||||||
|
printf("delta_psi_c %.4f ", (double)delta_psi_c);
|
||||||
|
printf("psi_c %.4f ", (double)psi_c);
|
||||||
|
printf("att.yaw %.4f ", (double)att.yaw);
|
||||||
|
printf("psi_e %.4f ", (double)psi_e);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* calculate roll setpoint, do this artificially around zero */
|
||||||
|
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
|
||||||
|
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
|
||||||
|
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
|
||||||
|
|
||||||
|
/* limit turn rate */
|
||||||
|
if (psi_rate_c > p.headingr_lim) {
|
||||||
|
psi_rate_c = p.headingr_lim;
|
||||||
|
|
||||||
|
} else if (psi_rate_c < -p.headingr_lim) {
|
||||||
|
psi_rate_c = -p.headingr_lim;
|
||||||
|
}
|
||||||
|
|
||||||
|
float psi_rate_e = psi_rate_c - att.yawspeed;
|
||||||
|
|
||||||
|
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
|
||||||
|
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
|
||||||
|
|
||||||
|
if (ground_speed < 10.0f) {
|
||||||
|
ground_speed = 10.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
|
||||||
|
|
||||||
|
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
|
||||||
|
|
||||||
|
if (verbose) {
|
||||||
|
printf("psi_rate_c %.4f ", (double)psi_rate_c);
|
||||||
|
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
|
||||||
|
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (verbose && counter % 100 == 0)
|
||||||
|
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (verbose && counter % 100 == 0)
|
||||||
|
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Very simple Altitude Control */
|
||||||
|
if (pos_updated) {
|
||||||
|
|
||||||
|
//TODO: take care of relative vs. ab. altitude
|
||||||
|
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// XXX need speed control
|
||||||
|
attitude_setpoint.thrust = 0.7f;
|
||||||
|
|
||||||
|
/* publish the attitude setpoint */
|
||||||
|
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
|
||||||
|
|
||||||
|
/* measure in what intervals the controller runs */
|
||||||
|
perf_count(fw_interval_perf);
|
||||||
|
|
||||||
|
counter++;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// XXX no setpoint, decent default needed (loiter?)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Very simple Altitude Control */
|
|
||||||
if(global_sp_updated_set_once && pos_updated)
|
|
||||||
{
|
|
||||||
|
|
||||||
//TODO: take care of relative vs. ab. altitude
|
|
||||||
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
|
|
||||||
|
|
||||||
}
|
|
||||||
/*Publish the attitude setpoint */
|
|
||||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
|
|
||||||
|
|
||||||
counter++;
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
printf("[fixedwing_pos_control] exiting.\n");
|
printf("[fixedwing_pos_control] exiting.\n");
|
||||||
thread_running = false;
|
thread_running = false;
|
||||||
|
|
||||||
|
|
||||||
close(attitude_setpoint_pub);
|
close(attitude_setpoint_pub);
|
||||||
|
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
exit(0);
|
exit(0);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -338,6 +421,7 @@ usage(const char *reason)
|
||||||
{
|
{
|
||||||
if (reason)
|
if (reason)
|
||||||
fprintf(stderr, "%s\n", reason);
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
|
||||||
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
|
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
@ -367,7 +451,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
|
||||||
deamon_task = task_spawn("fixedwing_pos_control",
|
deamon_task = task_spawn("fixedwing_pos_control",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_MAX - 20,
|
SCHED_PRIORITY_MAX - 20,
|
||||||
4096,
|
2048,
|
||||||
fixedwing_pos_control_thread_main,
|
fixedwing_pos_control_thread_main,
|
||||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
thread_running = true;
|
thread_running = true;
|
||||||
|
@ -382,9 +466,11 @@ int fixedwing_pos_control_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (thread_running) {
|
if (thread_running) {
|
||||||
printf("\tfixedwing_pos_control is running\n");
|
printf("\tfixedwing_pos_control is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("\tfixedwing_pos_control not started\n");
|
printf("\tfixedwing_pos_control not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,159 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_abs_f32.c
|
||||||
|
*
|
||||||
|
* Description: Vector absolute value.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup BasicAbs Vector Absolute Value
|
||||||
|
*
|
||||||
|
* Computes the absolute value of a vector on an element-by-element basis.
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = abs(pSrcA[n]), 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* The operation can be done in-place by setting the input and output pointers to the same buffer.
|
||||||
|
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAbs
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Floating-point vector absolute value.
|
||||||
|
* @param[in] *pSrc points to the input buffer
|
||||||
|
* @param[out] *pDst points to the output buffer
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_abs_f32(
|
||||||
|
float32_t * pSrc,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t in1, in2, in3, in4; /* temporary variables */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Calculate absolute and then store the results in the destination buffer. */
|
||||||
|
/* read sample from source */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
in1 = fabsf(in1);
|
||||||
|
|
||||||
|
/* read sample from source */
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
in2 = fabsf(in2);
|
||||||
|
|
||||||
|
/* read sample from source */
|
||||||
|
*pDst = in1;
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
in3 = fabsf(in3);
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
in4 = fabsf(in4);
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 1) = in2;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 2) = in3;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 3) = in4;
|
||||||
|
|
||||||
|
|
||||||
|
/* Update source pointer to process next sampels */
|
||||||
|
pSrc += 4u;
|
||||||
|
|
||||||
|
/* Update destination pointer to process next sampels */
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Calculate absolute and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = fabsf(*pSrc++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAbs group
|
||||||
|
*/
|
|
@ -0,0 +1,173 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_abs_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 vector absolute value.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAbs
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q15 vector absolute value.
|
||||||
|
* @param[in] *pSrc points to the input buffer
|
||||||
|
* @param[out] *pDst points to the output buffer
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_abs_q15(
|
||||||
|
q15_t * pSrc,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
q15_t in1; /* Input value1 */
|
||||||
|
q15_t in2; /* Input value2 */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Read two inputs */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
|
||||||
|
|
||||||
|
/* Store the Absolute result in the destination buffer by packing the two values, in a single cycle */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ =
|
||||||
|
__PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
|
||||||
|
((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ =
|
||||||
|
__PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
|
||||||
|
((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ =
|
||||||
|
__PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
|
||||||
|
((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ =
|
||||||
|
__PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
|
||||||
|
((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Read the input */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
|
||||||
|
/* Calculate absolute value of input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (in1 > 0) ? in1 : __QSUB16(0, in1);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
q15_t in; /* Temporary input variable */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Read the input */
|
||||||
|
in = *pSrc++;
|
||||||
|
|
||||||
|
/* Calculate absolute value of input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAbs group
|
||||||
|
*/
|
|
@ -0,0 +1,125 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_abs_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 vector absolute value.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAbs
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q31 vector absolute value.
|
||||||
|
* @param[in] *pSrc points to the input buffer
|
||||||
|
* @param[out] *pDst points to the output buffer
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_abs_q31(
|
||||||
|
q31_t * pSrc,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
q31_t in; /* Input value */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t in1, in2, in3, in4;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Calculate absolute of input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
in3 = *pSrc++;
|
||||||
|
in4 = *pSrc++;
|
||||||
|
|
||||||
|
*pDst++ = (in1 > 0) ? in1 : __QSUB(0, in1);
|
||||||
|
*pDst++ = (in2 > 0) ? in2 : __QSUB(0, in2);
|
||||||
|
*pDst++ = (in3 > 0) ? in3 : __QSUB(0, in3);
|
||||||
|
*pDst++ = (in4 > 0) ? in4 : __QSUB(0, in4);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Calculate absolute value of the input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
|
||||||
|
in = *pSrc++;
|
||||||
|
*pDst++ = (in > 0) ? in : ((in == 0x80000000) ? 0x7fffffff : -in);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAbs group
|
||||||
|
*/
|
|
@ -0,0 +1,152 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_abs_q7.c
|
||||||
|
*
|
||||||
|
* Description: Q7 vector absolute value.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAbs
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q7 vector absolute value.
|
||||||
|
* @param[in] *pSrc points to the input buffer
|
||||||
|
* @param[out] *pDst points to the output buffer
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* \par Conditions for optimum performance
|
||||||
|
* Input and output buffers should be aligned by 32-bit
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_abs_q7(
|
||||||
|
q7_t * pSrc,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
q7_t in; /* Input value1 */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t in1, in2, in3, in4; /* temporary input variables */
|
||||||
|
q31_t out1, out2, out3, out4; /* temporary output variables */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Read inputs */
|
||||||
|
in1 = (q31_t) * pSrc;
|
||||||
|
in2 = (q31_t) * (pSrc + 1);
|
||||||
|
in3 = (q31_t) * (pSrc + 2);
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
out1 = (in1 > 0) ? in1 : __QSUB8(0, in1);
|
||||||
|
|
||||||
|
/* read input */
|
||||||
|
in4 = (q31_t) * (pSrc + 3);
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
out2 = (in2 > 0) ? in2 : __QSUB8(0, in2);
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*pDst = (q7_t) out1;
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
out3 = (in3 > 0) ? in3 : __QSUB8(0, in3);
|
||||||
|
|
||||||
|
/* find absolute value */
|
||||||
|
out4 = (in4 > 0) ? in4 : __QSUB8(0, in4);
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 1) = (q7_t) out2;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 2) = (q7_t) out3;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 3) = (q7_t) out4;
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif // #define ARM_MATH_CM0
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = |A| */
|
||||||
|
/* Read the input */
|
||||||
|
in = *pSrc++;
|
||||||
|
|
||||||
|
/* Store the Absolute result in the destination buffer */
|
||||||
|
*pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? 0x7f : -in);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAbs group
|
||||||
|
*/
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_add_f32.c
|
||||||
|
*
|
||||||
|
* Description: Floating-point vector addition.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup BasicAdd Vector Addition
|
||||||
|
*
|
||||||
|
* Element-by-element addition of two vectors.
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = pSrcA[n] + pSrcB[n], 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAdd
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Floating-point vector addition.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_add_f32(
|
||||||
|
float32_t * pSrcA,
|
||||||
|
float32_t * pSrcB,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t inA1, inA2, inA3, inA4; /* temporary input variabels */
|
||||||
|
float32_t inB1, inB2, inB3, inB4; /* temporary input variables */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
|
||||||
|
/* read four inputs from sourceA and four inputs from sourceB */
|
||||||
|
inA1 = *pSrcA;
|
||||||
|
inB1 = *pSrcB;
|
||||||
|
inA2 = *(pSrcA + 1);
|
||||||
|
inB2 = *(pSrcB + 1);
|
||||||
|
inA3 = *(pSrcA + 2);
|
||||||
|
inB3 = *(pSrcB + 2);
|
||||||
|
inA4 = *(pSrcA + 3);
|
||||||
|
inB4 = *(pSrcB + 3);
|
||||||
|
|
||||||
|
/* C = A + B */
|
||||||
|
/* add and store result to destination */
|
||||||
|
*pDst = inA1 + inB1;
|
||||||
|
*(pDst + 1) = inA2 + inB2;
|
||||||
|
*(pDst + 2) = inA3 + inB3;
|
||||||
|
*(pDst + 3) = inA4 + inB4;
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrcA += 4u;
|
||||||
|
pSrcB += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrcA++) + (*pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAdd group
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_add_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 vector addition
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAdd
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q15 vector addition.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_add_q15(
|
||||||
|
q15_t * pSrcA,
|
||||||
|
q15_t * pSrcB,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2, inB1, inB2;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
inA1 = *__SIMD32(pSrcA)++;
|
||||||
|
inA2 = *__SIMD32(pSrcA)++;
|
||||||
|
inB1 = *__SIMD32(pSrcB)++;
|
||||||
|
inB2 = *__SIMD32(pSrcB)++;
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __QADD16(inA1, inB1);
|
||||||
|
*__SIMD32(pDst)++ = __QADD16(inA2, inB2);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) __QADD16(*pSrcA++, *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) __SSAT(((q31_t) * pSrcA++ + *pSrcB++), 16);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAdd group
|
||||||
|
*/
|
|
@ -0,0 +1,143 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_add_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 vector addition.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAdd
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q31 vector addition.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_add_q31(
|
||||||
|
q31_t * pSrcA,
|
||||||
|
q31_t * pSrcB,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2, inA3, inA4;
|
||||||
|
q31_t inB1, inB2, inB3, inB4;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
inA1 = *pSrcA++;
|
||||||
|
inA2 = *pSrcA++;
|
||||||
|
inB1 = *pSrcB++;
|
||||||
|
inB2 = *pSrcB++;
|
||||||
|
|
||||||
|
inA3 = *pSrcA++;
|
||||||
|
inA4 = *pSrcA++;
|
||||||
|
inB3 = *pSrcB++;
|
||||||
|
inB4 = *pSrcB++;
|
||||||
|
|
||||||
|
*pDst++ = __QADD(inA1, inB1);
|
||||||
|
*pDst++ = __QADD(inA2, inB2);
|
||||||
|
*pDst++ = __QADD(inA3, inB3);
|
||||||
|
*pDst++ = __QADD(inA4, inB4);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ + *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAdd group
|
||||||
|
*/
|
|
@ -0,0 +1,129 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_add_q7.c
|
||||||
|
*
|
||||||
|
* Description: Q7 vector addition.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicAdd
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q7 vector addition.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_add_q7(
|
||||||
|
q7_t * pSrcA,
|
||||||
|
q7_t * pSrcB,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT(*pSrcA++ + *pSrcB++, 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + B */
|
||||||
|
/* Add and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT((q15_t) * pSrcA++ + *pSrcB++, 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicAdd group
|
||||||
|
*/
|
|
@ -0,0 +1,125 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_dot_prod_f32.c
|
||||||
|
*
|
||||||
|
* Description: Floating-point dot product.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup dot_prod Vector Dot Product
|
||||||
|
*
|
||||||
|
* Computes the dot product of two vectors.
|
||||||
|
* The vectors are multiplied element-by-element and then summed.
|
||||||
|
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup dot_prod
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Dot product of floating-point vectors.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @param[out] *result output result returned here
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void arm_dot_prod_f32(
|
||||||
|
float32_t * pSrcA,
|
||||||
|
float32_t * pSrcB,
|
||||||
|
uint32_t blockSize,
|
||||||
|
float32_t * result)
|
||||||
|
{
|
||||||
|
float32_t sum = 0.0f; /* Temporary result storage */
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the result in a temporary buffer */
|
||||||
|
sum += (*pSrcA++) * (*pSrcB++);
|
||||||
|
sum += (*pSrcA++) * (*pSrcB++);
|
||||||
|
sum += (*pSrcA++) * (*pSrcB++);
|
||||||
|
sum += (*pSrcA++) * (*pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the result in a temporary buffer. */
|
||||||
|
sum += (*pSrcA++) * (*pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
/* Store the result back in the destination buffer */
|
||||||
|
*result = sum;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of dot_prod group
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_dot_prod_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 dot product.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup dot_prod
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Dot product of Q15 vectors.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @param[out] *result output result returned here
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The intermediate multiplications are in 1.15 x 1.15 = 2.30 format and these
|
||||||
|
* results are added to a 64-bit accumulator in 34.30 format.
|
||||||
|
* Nonsaturating additions are used and given that there are 33 guard bits in the accumulator
|
||||||
|
* there is no risk of overflow.
|
||||||
|
* The return result is in 34.30 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_dot_prod_q15(
|
||||||
|
q15_t * pSrcA,
|
||||||
|
q15_t * pSrcB,
|
||||||
|
uint32_t blockSize,
|
||||||
|
q63_t * result)
|
||||||
|
{
|
||||||
|
q63_t sum = 0; /* Temporary result storage */
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the result in a temporary buffer. */
|
||||||
|
sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
|
||||||
|
sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the results in a temporary buffer. */
|
||||||
|
sum = __SMLALD(*pSrcA++, *pSrcB++, sum);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the results in a temporary buffer. */
|
||||||
|
sum += (q63_t) ((q31_t) * pSrcA++ * *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
/* Store the result in the destination buffer in 34.30 format */
|
||||||
|
*result = sum;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of dot_prod group
|
||||||
|
*/
|
|
@ -0,0 +1,138 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_dot_prod_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 dot product.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup dot_prod
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Dot product of Q31 vectors.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @param[out] *result output result returned here
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The intermediate multiplications are in 1.31 x 1.31 = 2.62 format and these
|
||||||
|
* are truncated to 2.48 format by discarding the lower 14 bits.
|
||||||
|
* The 2.48 result is then added without saturation to a 64-bit accumulator in 16.48 format.
|
||||||
|
* There are 15 guard bits in the accumulator and there is no risk of overflow as long as
|
||||||
|
* the length of the vectors is less than 2^16 elements.
|
||||||
|
* The return result is in 16.48 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_dot_prod_q31(
|
||||||
|
q31_t * pSrcA,
|
||||||
|
q31_t * pSrcB,
|
||||||
|
uint32_t blockSize,
|
||||||
|
q63_t * result)
|
||||||
|
{
|
||||||
|
q63_t sum = 0; /* Temporary result storage */
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2, inA3, inA4;
|
||||||
|
q31_t inB1, inB2, inB3, inB4;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the result in a temporary buffer. */
|
||||||
|
inA1 = *pSrcA++;
|
||||||
|
inA2 = *pSrcA++;
|
||||||
|
inA3 = *pSrcA++;
|
||||||
|
inA4 = *pSrcA++;
|
||||||
|
inB1 = *pSrcB++;
|
||||||
|
inB2 = *pSrcB++;
|
||||||
|
inB3 = *pSrcB++;
|
||||||
|
inB4 = *pSrcB++;
|
||||||
|
|
||||||
|
sum += ((q63_t) inA1 * inB1) >> 14u;
|
||||||
|
sum += ((q63_t) inA2 * inB2) >> 14u;
|
||||||
|
sum += ((q63_t) inA3 * inB3) >> 14u;
|
||||||
|
sum += ((q63_t) inA4 * inB4) >> 14u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Calculate dot product and then store the result in a temporary buffer. */
|
||||||
|
sum += ((q63_t) * pSrcA++ * *pSrcB++) >> 14u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Store the result in the destination buffer in 16.48 format */
|
||||||
|
*result = sum;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of dot_prod group
|
||||||
|
*/
|
|
@ -0,0 +1,154 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_dot_prod_q7.c
|
||||||
|
*
|
||||||
|
* Description: Q7 dot product.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup dot_prod
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Dot product of Q7 vectors.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @param[out] *result output result returned here
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The intermediate multiplications are in 1.7 x 1.7 = 2.14 format and these
|
||||||
|
* results are added to an accumulator in 18.14 format.
|
||||||
|
* Nonsaturating additions are used and there is no danger of wrap around as long as
|
||||||
|
* the vectors are less than 2^18 elements long.
|
||||||
|
* The return result is in 18.14 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_dot_prod_q7(
|
||||||
|
q7_t * pSrcA,
|
||||||
|
q7_t * pSrcB,
|
||||||
|
uint32_t blockSize,
|
||||||
|
q31_t * result)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
q31_t sum = 0; /* Temporary variables to store output */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
q31_t input1, input2; /* Temporary variables to store input */
|
||||||
|
q31_t inA1, inA2, inB1, inB2; /* Temporary variables to store input */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* read 4 samples at a time from sourceA */
|
||||||
|
input1 = *__SIMD32(pSrcA)++;
|
||||||
|
/* read 4 samples at a time from sourceB */
|
||||||
|
input2 = *__SIMD32(pSrcB)++;
|
||||||
|
|
||||||
|
/* extract two q7_t samples to q15_t samples */
|
||||||
|
inA1 = __SXTB16(__ROR(input1, 8));
|
||||||
|
/* extract reminaing two samples */
|
||||||
|
inA2 = __SXTB16(input1);
|
||||||
|
/* extract two q7_t samples to q15_t samples */
|
||||||
|
inB1 = __SXTB16(__ROR(input2, 8));
|
||||||
|
/* extract reminaing two samples */
|
||||||
|
inB2 = __SXTB16(input2);
|
||||||
|
|
||||||
|
/* multiply and accumulate two samples at a time */
|
||||||
|
sum = __SMLAD(inA1, inB1, sum);
|
||||||
|
sum = __SMLAD(inA2, inB2, sum);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Dot product and then store the results in a temporary buffer. */
|
||||||
|
sum = __SMLAD(*pSrcA++, *pSrcB++, sum);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
|
||||||
|
/* Dot product and then store the results in a temporary buffer. */
|
||||||
|
sum += (q31_t) ((q15_t) * pSrcA++ * *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
/* Store the result in the destination buffer in 18.14 format */
|
||||||
|
*result = sum;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of dot_prod group
|
||||||
|
*/
|
|
@ -0,0 +1,172 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_mult_f32.c
|
||||||
|
*
|
||||||
|
* Description: Floating-point vector multiplication.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.5 2010/04/26
|
||||||
|
* incorporated review comments and updated with latest CMSIS layer
|
||||||
|
*
|
||||||
|
* Version 0.0.3 2010/03/10
|
||||||
|
* Initial version
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup BasicMult Vector Multiplication
|
||||||
|
*
|
||||||
|
* Element-by-element multiplication of two vectors.
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = pSrcA[n] * pSrcB[n], 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicMult
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Floating-point vector multiplication.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_mult_f32(
|
||||||
|
float32_t * pSrcA,
|
||||||
|
float32_t * pSrcB,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counters */
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t inA1, inA2, inA3, inA4; /* temporary input variables */
|
||||||
|
float32_t inB1, inB2, inB3, inB4; /* temporary input variables */
|
||||||
|
float32_t out1, out2, out3, out4; /* temporary output variables */
|
||||||
|
|
||||||
|
/* loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and store the results in output buffer */
|
||||||
|
/* read sample from sourceA */
|
||||||
|
inA1 = *pSrcA;
|
||||||
|
/* read sample from sourceB */
|
||||||
|
inB1 = *pSrcB;
|
||||||
|
/* read sample from sourceA */
|
||||||
|
inA2 = *(pSrcA + 1);
|
||||||
|
/* read sample from sourceB */
|
||||||
|
inB2 = *(pSrcB + 1);
|
||||||
|
|
||||||
|
/* out = sourceA * sourceB */
|
||||||
|
out1 = inA1 * inB1;
|
||||||
|
|
||||||
|
/* read sample from sourceA */
|
||||||
|
inA3 = *(pSrcA + 2);
|
||||||
|
/* read sample from sourceB */
|
||||||
|
inB3 = *(pSrcB + 2);
|
||||||
|
|
||||||
|
/* out = sourceA * sourceB */
|
||||||
|
out2 = inA2 * inB2;
|
||||||
|
|
||||||
|
/* read sample from sourceA */
|
||||||
|
inA4 = *(pSrcA + 3);
|
||||||
|
|
||||||
|
/* store result to destination buffer */
|
||||||
|
*pDst = out1;
|
||||||
|
|
||||||
|
/* read sample from sourceB */
|
||||||
|
inB4 = *(pSrcB + 3);
|
||||||
|
|
||||||
|
/* out = sourceA * sourceB */
|
||||||
|
out3 = inA3 * inB3;
|
||||||
|
|
||||||
|
/* store result to destination buffer */
|
||||||
|
*(pDst + 1) = out2;
|
||||||
|
|
||||||
|
/* out = sourceA * sourceB */
|
||||||
|
out4 = inA4 * inB4;
|
||||||
|
/* store result to destination buffer */
|
||||||
|
*(pDst + 2) = out3;
|
||||||
|
/* store result to destination buffer */
|
||||||
|
*(pDst + 3) = out4;
|
||||||
|
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrcA += 4u;
|
||||||
|
pSrcB += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and store the results in output buffer */
|
||||||
|
*pDst++ = (*pSrcA++) * (*pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicMult group
|
||||||
|
*/
|
|
@ -0,0 +1,152 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_mult_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 vector multiplication.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.5 2010/04/26
|
||||||
|
* incorporated review comments and updated with latest CMSIS layer
|
||||||
|
*
|
||||||
|
* Version 0.0.3 2010/03/10
|
||||||
|
* Initial version
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicMult
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q15 vector multiplication
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_mult_q15(
|
||||||
|
q15_t * pSrcA,
|
||||||
|
q15_t * pSrcB,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counters */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2, inB1, inB2; /* temporary input variables */
|
||||||
|
q15_t out1, out2, out3, out4; /* temporary output variables */
|
||||||
|
q31_t mul1, mul2, mul3, mul4; /* temporary variables */
|
||||||
|
|
||||||
|
/* loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* read two samples at a time from sourceA */
|
||||||
|
inA1 = *__SIMD32(pSrcA)++;
|
||||||
|
/* read two samples at a time from sourceB */
|
||||||
|
inB1 = *__SIMD32(pSrcB)++;
|
||||||
|
/* read two samples at a time from sourceA */
|
||||||
|
inA2 = *__SIMD32(pSrcA)++;
|
||||||
|
/* read two samples at a time from sourceB */
|
||||||
|
inB2 = *__SIMD32(pSrcB)++;
|
||||||
|
|
||||||
|
/* multiply mul = sourceA * sourceB */
|
||||||
|
mul1 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
|
||||||
|
mul2 = (q31_t) ((q15_t) inA1 * (q15_t) inB1);
|
||||||
|
mul3 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB2 >> 16));
|
||||||
|
mul4 = (q31_t) ((q15_t) inA2 * (q15_t) inB2);
|
||||||
|
|
||||||
|
/* saturate result to 16 bit */
|
||||||
|
out1 = (q15_t) __SSAT(mul1 >> 15, 16);
|
||||||
|
out2 = (q15_t) __SSAT(mul2 >> 15, 16);
|
||||||
|
out3 = (q15_t) __SSAT(mul3 >> 15, 16);
|
||||||
|
out4 = (q15_t) __SSAT(mul4 >> 15, 16);
|
||||||
|
|
||||||
|
/* store the result */
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
|
||||||
|
|
||||||
|
#endif // #ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and store the result in the destination buffer */
|
||||||
|
*pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicMult group
|
||||||
|
*/
|
|
@ -0,0 +1,143 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_mult_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 vector multiplication.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.5 2010/04/26
|
||||||
|
* incorporated review comments and updated with latest CMSIS layer
|
||||||
|
*
|
||||||
|
* Version 0.0.3 2010/03/10
|
||||||
|
* Initial version
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicMult
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q31 vector multiplication.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_mult_q31(
|
||||||
|
q31_t * pSrcA,
|
||||||
|
q31_t * pSrcB,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counters */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2, inA3, inA4; /* temporary input variables */
|
||||||
|
q31_t inB1, inB2, inB3, inB4; /* temporary input variables */
|
||||||
|
q31_t out1, out2, out3, out4; /* temporary output variables */
|
||||||
|
|
||||||
|
/* loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and then store the results in the destination buffer. */
|
||||||
|
inA1 = *pSrcA++;
|
||||||
|
inA2 = *pSrcA++;
|
||||||
|
inA3 = *pSrcA++;
|
||||||
|
inA4 = *pSrcA++;
|
||||||
|
inB1 = *pSrcB++;
|
||||||
|
inB2 = *pSrcB++;
|
||||||
|
inB3 = *pSrcB++;
|
||||||
|
inB4 = *pSrcB++;
|
||||||
|
|
||||||
|
out1 = ((q63_t) inA1 * inB1) >> 32;
|
||||||
|
out2 = ((q63_t) inA2 * inB2) >> 32;
|
||||||
|
out3 = ((q63_t) inA3 * inB3) >> 32;
|
||||||
|
out4 = ((q63_t) inA4 * inB4) >> 32;
|
||||||
|
|
||||||
|
out1 = __SSAT(out1, 31);
|
||||||
|
out2 = __SSAT(out2, 31);
|
||||||
|
out3 = __SSAT(out3, 31);
|
||||||
|
out4 = __SSAT(out4, 31);
|
||||||
|
|
||||||
|
*pDst++ = out1 << 1u;
|
||||||
|
*pDst++ = out2 << 1u;
|
||||||
|
*pDst++ = out3 << 1u;
|
||||||
|
*pDst++ = out4 << 1u;
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and then store the results in the destination buffer. */
|
||||||
|
*pDst++ =
|
||||||
|
(q31_t) clip_q63_to_q31(((q63_t) (*pSrcA++) * (*pSrcB++)) >> 31);
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicMult group
|
||||||
|
*/
|
|
@ -0,0 +1,128 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_mult_q7.c
|
||||||
|
*
|
||||||
|
* Description: Q7 vector multiplication.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
*
|
||||||
|
* Version 0.0.5 2010/04/26
|
||||||
|
* incorporated review comments and updated with latest CMSIS layer
|
||||||
|
*
|
||||||
|
* Version 0.0.3 2010/03/10 DP
|
||||||
|
* Initial version
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicMult
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q7 vector multiplication
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_mult_q7(
|
||||||
|
q7_t * pSrcA,
|
||||||
|
q7_t * pSrcB,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counters */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q7_t out1, out2, out3, out4; /* Temporary variables to store the product */
|
||||||
|
|
||||||
|
/* loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and store the results in temporary variables */
|
||||||
|
out1 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
|
||||||
|
out2 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
|
||||||
|
out3 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
|
||||||
|
out4 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
|
||||||
|
|
||||||
|
/* Store the results of 4 inputs in the destination buffer in single cycle by packing */
|
||||||
|
*__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * B */
|
||||||
|
/* Multiply the inputs and store the result in the destination buffer */
|
||||||
|
*pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
|
||||||
|
|
||||||
|
/* Decrement the blockSize loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicMult group
|
||||||
|
*/
|
|
@ -0,0 +1,137 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_negate_f32.c
|
||||||
|
*
|
||||||
|
* Description: Negates floating-point vectors.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup negate Vector Negate
|
||||||
|
*
|
||||||
|
* Negates the elements of a vector.
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = -pSrc[n], 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup negate
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Negates the elements of a floating-point vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_negate_f32(
|
||||||
|
float32_t * pSrc,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t in1, in2, in3, in4; /* temporary variables */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* read inputs from source */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* negate the input */
|
||||||
|
in1 = -in1;
|
||||||
|
in2 = -in2;
|
||||||
|
in3 = -in3;
|
||||||
|
in4 = -in4;
|
||||||
|
|
||||||
|
/* store the result to destination */
|
||||||
|
*pDst = in1;
|
||||||
|
*(pDst + 1) = in2;
|
||||||
|
*(pDst + 2) = in3;
|
||||||
|
*(pDst + 3) = in4;
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Negate and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = -*pSrc++;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of negate group
|
||||||
|
*/
|
|
@ -0,0 +1,137 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_negate_q15.c
|
||||||
|
*
|
||||||
|
* Description: Negates Q15 vectors.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup negate
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Negates the elements of a Q15 vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* \par Conditions for optimum performance
|
||||||
|
* Input and output buffers should be aligned by 32-bit
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_negate_q15(
|
||||||
|
q15_t * pSrc,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
q15_t in;
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
q31_t in1, in2; /* Temporary variables */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Read two inputs at a time */
|
||||||
|
in1 = _SIMD32_OFFSET(pSrc);
|
||||||
|
in2 = _SIMD32_OFFSET(pSrc + 2);
|
||||||
|
|
||||||
|
/* negate two samples at a time */
|
||||||
|
in1 = __QSUB16(0, in1);
|
||||||
|
|
||||||
|
/* negate two samples at a time */
|
||||||
|
in2 = __QSUB16(0, in2);
|
||||||
|
|
||||||
|
/* store the result to destination 2 samples at a time */
|
||||||
|
_SIMD32_OFFSET(pDst) = in1;
|
||||||
|
/* store the result to destination 2 samples at a time */
|
||||||
|
_SIMD32_OFFSET(pDst + 2) = in2;
|
||||||
|
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Negate and then store the result in the destination buffer. */
|
||||||
|
in = *pSrc++;
|
||||||
|
*pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of negate group
|
||||||
|
*/
|
|
@ -0,0 +1,124 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_negate_q31.c
|
||||||
|
*
|
||||||
|
* Description: Negates Q31 vectors.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup negate
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Negates the elements of a Q31 vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_negate_q31(
|
||||||
|
q31_t * pSrc,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
q31_t in; /* Temporary variable */
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t in1, in2, in3, in4;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Negate and then store the results in the destination buffer. */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
in3 = *pSrc++;
|
||||||
|
in4 = *pSrc++;
|
||||||
|
|
||||||
|
*pDst++ = __QSUB(0, in1);
|
||||||
|
*pDst++ = __QSUB(0, in2);
|
||||||
|
*pDst++ = __QSUB(0, in3);
|
||||||
|
*pDst++ = __QSUB(0, in4);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Negate and then store the result in the destination buffer. */
|
||||||
|
in = *pSrc++;
|
||||||
|
*pDst++ = (in == 0x80000000) ? 0x7fffffff : -in;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of negate group
|
||||||
|
*/
|
|
@ -0,0 +1,120 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_negate_q7.c
|
||||||
|
*
|
||||||
|
* Description: Negates Q7 vectors.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup negate
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Negates the elements of a Q7 vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_negate_q7(
|
||||||
|
q7_t * pSrc,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
q7_t in;
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t input; /* Input values1-4 */
|
||||||
|
q31_t zero = 0x00000000;
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Read four inputs */
|
||||||
|
input = *__SIMD32(pSrc)++;
|
||||||
|
|
||||||
|
/* Store the Negated results in the destination buffer in a single cycle by packing the results */
|
||||||
|
*__SIMD32(pDst)++ = __QSUB8(zero, input);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = -A */
|
||||||
|
/* Negate and then store the results in the destination buffer. */ \
|
||||||
|
in = *pSrc++;
|
||||||
|
*pDst++ = (in == (q7_t) 0x80) ? 0x7f : -in;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of negate group
|
||||||
|
*/
|
|
@ -0,0 +1,158 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_offset_f32.c
|
||||||
|
*
|
||||||
|
* Description: Floating-point vector offset.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup offset Vector Offset
|
||||||
|
*
|
||||||
|
* Adds a constant offset to each element of a vector.
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = pSrc[n] + offset, 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup offset
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Adds a constant offset to a floating-point vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] offset is the offset to be added
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void arm_offset_f32(
|
||||||
|
float32_t * pSrc,
|
||||||
|
float32_t offset,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t in1, in2, in3, in4;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the results in the destination buffer. */
|
||||||
|
/* read samples from source */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
|
||||||
|
/* add offset to input */
|
||||||
|
in1 = in1 + offset;
|
||||||
|
|
||||||
|
/* read samples from source */
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
|
||||||
|
/* add offset to input */
|
||||||
|
in2 = in2 + offset;
|
||||||
|
|
||||||
|
/* read samples from source */
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* add offset to input */
|
||||||
|
in3 = in3 + offset;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*pDst = in1;
|
||||||
|
|
||||||
|
/* add offset to input */
|
||||||
|
in4 = in4 + offset;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 1) = in2;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 2) = in3;
|
||||||
|
|
||||||
|
/* store result to destination */
|
||||||
|
*(pDst + 3) = in4;
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrc++) + offset;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of offset group
|
||||||
|
*/
|
|
@ -0,0 +1,131 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_offset_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 vector offset.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup offset
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Adds a constant offset to a Q15 vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] offset is the offset to be added
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_offset_q15(
|
||||||
|
q15_t * pSrc,
|
||||||
|
q15_t offset,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t offset_packed; /* Offset packed to 32 bit */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* Offset is packed to 32 bit in order to use SIMD32 for addition */
|
||||||
|
offset_packed = __PKHBT(offset, offset, 16);
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the results in the destination buffer, 2 samples at a time. */
|
||||||
|
*__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
|
||||||
|
*__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) __QADD16(*pSrc++, offset);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) __SSAT(((q31_t) * pSrc++ + offset), 16);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of offset group
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_offset_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 vector offset.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup offset
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Adds a constant offset to a Q31 vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] offset is the offset to be added
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_offset_q31(
|
||||||
|
q31_t * pSrc,
|
||||||
|
q31_t offset,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t in1, in2, in3, in4;
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the results in the destination buffer. */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
in3 = *pSrc++;
|
||||||
|
in4 = *pSrc++;
|
||||||
|
|
||||||
|
*pDst++ = __QADD(in1, offset);
|
||||||
|
*pDst++ = __QADD(in2, offset);
|
||||||
|
*pDst++ = __QADD(in3, offset);
|
||||||
|
*pDst++ = __QADD(in4, offset);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = __QADD(*pSrc++, offset);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of offset group
|
||||||
|
*/
|
|
@ -0,0 +1,130 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_offset_q7.c
|
||||||
|
*
|
||||||
|
* Description: Q7 vector offset.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup offset
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Adds a constant offset to a Q7 vector.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] offset is the offset to be added
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q7 range [0x80 0x7F] are saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_offset_q7(
|
||||||
|
q7_t * pSrc,
|
||||||
|
q7_t offset,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t offset_packed; /* Offset packed to 32 bit */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* Offset is packed to 32 bit in order to use SIMD32 for addition */
|
||||||
|
offset_packed = __PACKq7(offset, offset, offset, offset);
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the results in the destination bufferfor 4 samples at a time. */
|
||||||
|
*__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrc)++, offset_packed);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT(*pSrc++ + offset, 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A + offset */
|
||||||
|
/* Add offset and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT((q15_t) * pSrc++ + offset, 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of offset group
|
||||||
|
*/
|
|
@ -0,0 +1,161 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_scale_f32.c
|
||||||
|
*
|
||||||
|
* Description: Multiplies a floating-point vector by a scalar.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup scale Vector Scale
|
||||||
|
*
|
||||||
|
* Multiply a vector by a scalar value. For floating-point data, the algorithm used is:
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = pSrc[n] * scale, 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* In the fixed-point Q7, Q15, and Q31 functions, <code>scale</code> is represented by
|
||||||
|
* a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>.
|
||||||
|
* The shift allows the gain of the scaling operation to exceed 1.0.
|
||||||
|
* The algorithm used with fixed-point data is:
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = (pSrc[n] * scaleFract) << shift, 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* The overall scale factor applied to the fixed-point data is
|
||||||
|
* <pre>
|
||||||
|
* scale = scaleFract * 2^shift.
|
||||||
|
* </pre>
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup scale
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Multiplies a floating-point vector by a scalar.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] scale scale factor to be applied
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void arm_scale_f32(
|
||||||
|
float32_t * pSrc,
|
||||||
|
float32_t scale,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t in1, in2, in3, in4; /* temporary variabels */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the results in the destination buffer. */
|
||||||
|
/* read input samples from source */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
|
||||||
|
/* multiply with scaling factor */
|
||||||
|
in1 = in1 * scale;
|
||||||
|
|
||||||
|
/* read input sample from source */
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
|
||||||
|
/* multiply with scaling factor */
|
||||||
|
in2 = in2 * scale;
|
||||||
|
|
||||||
|
/* read input sample from source */
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* multiply with scaling factor */
|
||||||
|
in3 = in3 * scale;
|
||||||
|
in4 = in4 * scale;
|
||||||
|
/* store the result to destination */
|
||||||
|
*pDst = in1;
|
||||||
|
*(pDst + 1) = in2;
|
||||||
|
*(pDst + 2) = in3;
|
||||||
|
*(pDst + 3) = in4;
|
||||||
|
|
||||||
|
/* update pointers to process next samples */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrc++) * scale;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of scale group
|
||||||
|
*/
|
|
@ -0,0 +1,157 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_scale_q15.c
|
||||||
|
*
|
||||||
|
* Description: Multiplies a Q15 vector by a scalar.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup scale
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Multiplies a Q15 vector by a scalar.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] scaleFract fractional portion of the scale value
|
||||||
|
* @param[in] shift number of bits to shift the result by
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format.
|
||||||
|
* These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void arm_scale_q15(
|
||||||
|
q15_t * pSrc,
|
||||||
|
q15_t scaleFract,
|
||||||
|
int8_t shift,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
int8_t kShift = 15 - shift; /* shift to apply after scaling */
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q15_t in1, in2, in3, in4;
|
||||||
|
q31_t inA1, inA2; /* Temporary variables */
|
||||||
|
q31_t out1, out2, out3, out4;
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* Reading 2 inputs from memory */
|
||||||
|
inA1 = *__SIMD32(pSrc)++;
|
||||||
|
inA2 = *__SIMD32(pSrc)++;
|
||||||
|
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the inputs and then store the 2 results in the destination buffer
|
||||||
|
* in single cycle by packing the outputs */
|
||||||
|
out1 = (q31_t) ((q15_t) (inA1 >> 16) * scaleFract);
|
||||||
|
out2 = (q31_t) ((q15_t) inA1 * scaleFract);
|
||||||
|
out3 = (q31_t) ((q15_t) (inA2 >> 16) * scaleFract);
|
||||||
|
out4 = (q31_t) ((q15_t) inA2 * scaleFract);
|
||||||
|
|
||||||
|
/* apply shifting */
|
||||||
|
out1 = out1 >> kShift;
|
||||||
|
out2 = out2 >> kShift;
|
||||||
|
out3 = out3 >> kShift;
|
||||||
|
out4 = out4 >> kShift;
|
||||||
|
|
||||||
|
/* saturate the output */
|
||||||
|
in1 = (q15_t) (__SSAT(out1, 16));
|
||||||
|
in2 = (q15_t) (__SSAT(out2, 16));
|
||||||
|
in3 = (q15_t) (__SSAT(out3, 16));
|
||||||
|
in4 = (q15_t) (__SSAT(out4, 16));
|
||||||
|
|
||||||
|
/* store the result to destination */
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(in2, in1, 16);
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(in4, in3, 16);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 16));
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) (__SSAT(((q31_t) * pSrc++ * scaleFract) >> kShift, 16));
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of scale group
|
||||||
|
*/
|
|
@ -0,0 +1,221 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_scale_q31.c
|
||||||
|
*
|
||||||
|
* Description: Multiplies a Q31 vector by a scalar.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup scale
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Multiplies a Q31 vector by a scalar.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] scaleFract fractional portion of the scale value
|
||||||
|
* @param[in] shift number of bits to shift the result by
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format.
|
||||||
|
* These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_scale_q31(
|
||||||
|
q31_t * pSrc,
|
||||||
|
q31_t scaleFract,
|
||||||
|
int8_t shift,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
int8_t kShift = shift + 1; /* Shift to apply after scaling */
|
||||||
|
int8_t sign = (kShift & 0x80);
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
q31_t in, out;
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
q31_t in1, in2, in3, in4; /* temporary input variables */
|
||||||
|
q31_t out1, out2, out3, out4; /* temporary output variabels */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
if(sign == 0u)
|
||||||
|
{
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* read four inputs from source */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* multiply input with scaler value */
|
||||||
|
in1 = ((q63_t) in1 * scaleFract) >> 32;
|
||||||
|
in2 = ((q63_t) in2 * scaleFract) >> 32;
|
||||||
|
in3 = ((q63_t) in3 * scaleFract) >> 32;
|
||||||
|
in4 = ((q63_t) in4 * scaleFract) >> 32;
|
||||||
|
|
||||||
|
/* apply shifting */
|
||||||
|
out1 = in1 << kShift;
|
||||||
|
out2 = in2 << kShift;
|
||||||
|
|
||||||
|
/* saturate the results. */
|
||||||
|
if(in1 != (out1 >> kShift))
|
||||||
|
out1 = 0x7FFFFFFF ^ (in1 >> 31);
|
||||||
|
|
||||||
|
if(in2 != (out2 >> kShift))
|
||||||
|
out2 = 0x7FFFFFFF ^ (in2 >> 31);
|
||||||
|
|
||||||
|
out3 = in3 << kShift;
|
||||||
|
out4 = in4 << kShift;
|
||||||
|
|
||||||
|
*pDst = out1;
|
||||||
|
*(pDst + 1) = out2;
|
||||||
|
|
||||||
|
if(in3 != (out3 >> kShift))
|
||||||
|
out3 = 0x7FFFFFFF ^ (in3 >> 31);
|
||||||
|
|
||||||
|
if(in4 != (out4 >> kShift))
|
||||||
|
out4 = 0x7FFFFFFF ^ (in4 >> 31);
|
||||||
|
|
||||||
|
/* Store result destination */
|
||||||
|
*(pDst + 2) = out3;
|
||||||
|
*(pDst + 3) = out4;
|
||||||
|
|
||||||
|
/* Update pointers to process next sampels */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
kShift = -kShift;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* read four inputs from source */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* multiply input with scaler value */
|
||||||
|
in1 = ((q63_t) in1 * scaleFract) >> 32;
|
||||||
|
in2 = ((q63_t) in2 * scaleFract) >> 32;
|
||||||
|
in3 = ((q63_t) in3 * scaleFract) >> 32;
|
||||||
|
in4 = ((q63_t) in4 * scaleFract) >> 32;
|
||||||
|
|
||||||
|
/* apply shifting */
|
||||||
|
out1 = in1 >> kShift;
|
||||||
|
out2 = in2 >> kShift;
|
||||||
|
|
||||||
|
out3 = in3 >> kShift;
|
||||||
|
out4 = in4 >> kShift;
|
||||||
|
|
||||||
|
/* Store result destination */
|
||||||
|
*pDst = out1;
|
||||||
|
*(pDst + 1) = out2;
|
||||||
|
|
||||||
|
*(pDst + 2) = out3;
|
||||||
|
*(pDst + 3) = out4;
|
||||||
|
|
||||||
|
/* Update pointers to process next sampels */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the result in the destination buffer. */
|
||||||
|
in = *pSrc++;
|
||||||
|
in = ((q63_t) in * scaleFract) >> 32;
|
||||||
|
|
||||||
|
if(sign == 0)
|
||||||
|
{
|
||||||
|
out = in << kShift;
|
||||||
|
if(in != (out >> kShift))
|
||||||
|
out = 0x7FFFFFFF ^ (in >> 31);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
out = in >> kShift;
|
||||||
|
}
|
||||||
|
|
||||||
|
*pDst++ = out;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of scale group
|
||||||
|
*/
|
|
@ -0,0 +1,144 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_scale_q7.c
|
||||||
|
*
|
||||||
|
* Description: Multiplies a Q7 vector by a scalar.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup scale
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Multiplies a Q7 vector by a scalar.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] scaleFract fractional portion of the scale value
|
||||||
|
* @param[in] shift number of bits to shift the result by
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.7 format.
|
||||||
|
* These are multiplied to yield a 2.14 intermediate result and this is shifted with saturation to 1.7 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_scale_q7(
|
||||||
|
q7_t * pSrc,
|
||||||
|
q7_t scaleFract,
|
||||||
|
int8_t shift,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
int8_t kShift = 7 - shift; /* shift to apply after scaling */
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q7_t in1, in2, in3, in4, out1, out2, out3, out4; /* Temporary variables to store input & output */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* Reading 4 inputs from memory */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
in3 = *pSrc++;
|
||||||
|
in4 = *pSrc++;
|
||||||
|
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the inputs and then store the results in the temporary variables. */
|
||||||
|
out1 = (q7_t) (__SSAT(((in1) * scaleFract) >> kShift, 8));
|
||||||
|
out2 = (q7_t) (__SSAT(((in2) * scaleFract) >> kShift, 8));
|
||||||
|
out3 = (q7_t) (__SSAT(((in3) * scaleFract) >> kShift, 8));
|
||||||
|
out4 = (q7_t) (__SSAT(((in4) * scaleFract) >> kShift, 8));
|
||||||
|
|
||||||
|
/* Packing the individual outputs into 32bit and storing in
|
||||||
|
* destination buffer in single write */
|
||||||
|
*__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 8));
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A * scale */
|
||||||
|
/* Scale the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) (__SSAT((((q15_t) * pSrc++ * scaleFract) >> kShift), 8));
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of scale group
|
||||||
|
*/
|
|
@ -0,0 +1,243 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_shift_q15.c
|
||||||
|
*
|
||||||
|
* Description: Shifts the elements of a Q15 vector by a specified number of bits.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup shift
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Shifts the elements of a Q15 vector a specified number of bits.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_shift_q15(
|
||||||
|
q15_t * pSrc,
|
||||||
|
int8_t shiftBits,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
uint8_t sign; /* Sign of shiftBits */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
q15_t in1, in2; /* Temporary variables */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* Getting the sign of shiftBits */
|
||||||
|
sign = (shiftBits & 0x80);
|
||||||
|
|
||||||
|
/* If the shift value is positive then do right shift else left shift */
|
||||||
|
if(sign == 0u)
|
||||||
|
{
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* Read 2 inputs */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Shift the inputs and then store the results in the destination buffer. */
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
|
||||||
|
__SSAT((in2 << shiftBits), 16), 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
|
||||||
|
__SSAT((in1 << shiftBits), 16), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
|
||||||
|
__SSAT((in2 << shiftBits), 16), 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
|
||||||
|
__SSAT((in1 << shiftBits), 16), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Shift and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = __SSAT((*pSrc++ << shiftBits), 16);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* Read 2 inputs */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Shift the inputs and then store the results in the destination buffer. */
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
|
||||||
|
(in2 >> -shiftBits), 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
|
||||||
|
(in1 >> -shiftBits), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
in1 = *pSrc++;
|
||||||
|
in2 = *pSrc++;
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
|
||||||
|
(in2 >> -shiftBits), 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
|
||||||
|
(in1 >> -shiftBits), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Shift the inputs and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrc++ >> -shiftBits);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Getting the sign of shiftBits */
|
||||||
|
sign = (shiftBits & 0x80);
|
||||||
|
|
||||||
|
/* If the shift value is positive then do right shift else left shift */
|
||||||
|
if(sign == 0u)
|
||||||
|
{
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Shift and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = __SSAT(((q31_t) * pSrc++ << shiftBits), 16);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Shift the inputs and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrc++ >> -shiftBits);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of shift group
|
||||||
|
*/
|
|
@ -0,0 +1,195 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_shift_q31.c
|
||||||
|
*
|
||||||
|
* Description: Shifts the elements of a Q31 vector by a specified number of bits.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
/**
|
||||||
|
* @defgroup shift Vector Shift
|
||||||
|
*
|
||||||
|
* Shifts the elements of a fixed-point vector by a specified number of bits.
|
||||||
|
* There are separate functions for Q7, Q15, and Q31 data types.
|
||||||
|
* The underlying algorithm used is:
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = pSrc[n] << shift, 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* If <code>shift</code> is positive then the elements of the vector are shifted to the left.
|
||||||
|
* If <code>shift</code> is negative then the elements of the vector are shifted to the right.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup shift
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Shifts the elements of a Q31 vector a specified number of bits.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_shift_q31(
|
||||||
|
q31_t * pSrc,
|
||||||
|
int8_t shiftBits,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
q31_t in1, in2, in3, in4; /* Temporary input variables */
|
||||||
|
q31_t out1, out2, out3, out4; /* Temporary output variables */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
|
||||||
|
if(sign == 0u)
|
||||||
|
{
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Shift the input and then store the results in the destination buffer. */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
out1 = in1 << shiftBits;
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
out2 = in2 << shiftBits;
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
if(in1 != (out1 >> shiftBits))
|
||||||
|
out1 = 0x7FFFFFFF ^ (in1 >> 31);
|
||||||
|
|
||||||
|
if(in2 != (out2 >> shiftBits))
|
||||||
|
out2 = 0x7FFFFFFF ^ (in2 >> 31);
|
||||||
|
|
||||||
|
*pDst = out1;
|
||||||
|
out3 = in3 << shiftBits;
|
||||||
|
*(pDst + 1) = out2;
|
||||||
|
out4 = in4 << shiftBits;
|
||||||
|
|
||||||
|
if(in3 != (out3 >> shiftBits))
|
||||||
|
out3 = 0x7FFFFFFF ^ (in3 >> 31);
|
||||||
|
|
||||||
|
if(in4 != (out4 >> shiftBits))
|
||||||
|
out4 = 0x7FFFFFFF ^ (in4 >> 31);
|
||||||
|
|
||||||
|
*(pDst + 2) = out3;
|
||||||
|
*(pDst + 3) = out4;
|
||||||
|
|
||||||
|
/* Update destination pointer to process next sampels */
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Shift the input and then store the results in the destination buffer. */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
*pDst = (in1 >> -shiftBits);
|
||||||
|
*(pDst + 1) = (in2 >> -shiftBits);
|
||||||
|
*(pDst + 2) = (in3 >> -shiftBits);
|
||||||
|
*(pDst + 3) = (in4 >> -shiftBits);
|
||||||
|
|
||||||
|
|
||||||
|
pSrc += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A (>> or <<) shiftBits */
|
||||||
|
/* Shift the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (sign == 0u) ? clip_q63_to_q31((q63_t) * pSrc++ << shiftBits) :
|
||||||
|
(*pSrc++ >> -shiftBits);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of shift group
|
||||||
|
*/
|
|
@ -0,0 +1,215 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_shift_q7.c
|
||||||
|
*
|
||||||
|
* Description: Processing function for the Q7 Shifting
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup shift
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Shifts the elements of a Q7 vector a specified number of bits.
|
||||||
|
* @param[in] *pSrc points to the input vector
|
||||||
|
* @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in the vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* \par Conditions for optimum performance
|
||||||
|
* Input and output buffers should be aligned by 32-bit
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q7 range [0x8 0x7F] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_shift_q7(
|
||||||
|
q7_t * pSrc,
|
||||||
|
int8_t shiftBits,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
uint8_t sign; /* Sign of shiftBits */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q7_t in1; /* Input value1 */
|
||||||
|
q7_t in2; /* Input value2 */
|
||||||
|
q7_t in3; /* Input value3 */
|
||||||
|
q7_t in4; /* Input value4 */
|
||||||
|
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* Getting the sign of shiftBits */
|
||||||
|
sign = (shiftBits & 0x80);
|
||||||
|
|
||||||
|
/* If the shift value is positive then do right shift else left shift */
|
||||||
|
if(sign == 0u)
|
||||||
|
{
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Read 4 inputs */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* Store the Shifted result in the destination buffer in single cycle by packing the outputs */
|
||||||
|
*__SIMD32(pDst)++ = __PACKq7(__SSAT((in1 << shiftBits), 8),
|
||||||
|
__SSAT((in2 << shiftBits), 8),
|
||||||
|
__SSAT((in3 << shiftBits), 8),
|
||||||
|
__SSAT((in4 << shiftBits), 8));
|
||||||
|
/* Update source pointer to process next sampels */
|
||||||
|
pSrc += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Shift the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT((*pSrc++ << shiftBits), 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
shiftBits = -shiftBits;
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Read 4 inputs */
|
||||||
|
in1 = *pSrc;
|
||||||
|
in2 = *(pSrc + 1);
|
||||||
|
in3 = *(pSrc + 2);
|
||||||
|
in4 = *(pSrc + 3);
|
||||||
|
|
||||||
|
/* Store the Shifted result in the destination buffer in single cycle by packing the outputs */
|
||||||
|
*__SIMD32(pDst)++ = __PACKq7((in1 >> shiftBits), (in2 >> shiftBits),
|
||||||
|
(in3 >> shiftBits), (in4 >> shiftBits));
|
||||||
|
|
||||||
|
|
||||||
|
pSrc += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Shift the input and then store the result in the destination buffer. */
|
||||||
|
in1 = *pSrc++;
|
||||||
|
*pDst++ = (in1 >> shiftBits);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Getting the sign of shiftBits */
|
||||||
|
sign = (shiftBits & 0x80);
|
||||||
|
|
||||||
|
/* If the shift value is positive then do right shift else left shift */
|
||||||
|
if(sign == 0u)
|
||||||
|
{
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A << shiftBits */
|
||||||
|
/* Shift the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT(((q15_t) * pSrc++ << shiftBits), 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A >> shiftBits */
|
||||||
|
/* Shift the input and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrc++ >> -shiftBits);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of shift group
|
||||||
|
*/
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sub_f32.c
|
||||||
|
*
|
||||||
|
* Description: Floating-point vector subtraction.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* ---------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup BasicSub Vector Subtraction
|
||||||
|
*
|
||||||
|
* Element-by-element subtraction of two vectors.
|
||||||
|
*
|
||||||
|
* <pre>
|
||||||
|
* pDst[n] = pSrcA[n] - pSrcB[n], 0 <= n < blockSize.
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicSub
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Floating-point vector subtraction.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_sub_f32(
|
||||||
|
float32_t * pSrcA,
|
||||||
|
float32_t * pSrcB,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
float32_t inA1, inA2, inA3, inA4; /* temporary variables */
|
||||||
|
float32_t inB1, inB2, inB3, inB4; /* temporary variables */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the results in the destination buffer. */
|
||||||
|
/* Read 4 input samples from sourceA and sourceB */
|
||||||
|
inA1 = *pSrcA;
|
||||||
|
inB1 = *pSrcB;
|
||||||
|
inA2 = *(pSrcA + 1);
|
||||||
|
inB2 = *(pSrcB + 1);
|
||||||
|
inA3 = *(pSrcA + 2);
|
||||||
|
inB3 = *(pSrcB + 2);
|
||||||
|
inA4 = *(pSrcA + 3);
|
||||||
|
inB4 = *(pSrcB + 3);
|
||||||
|
|
||||||
|
/* dst = srcA - srcB */
|
||||||
|
/* subtract and store the result */
|
||||||
|
*pDst = inA1 - inB1;
|
||||||
|
*(pDst + 1) = inA2 - inB2;
|
||||||
|
*(pDst + 2) = inA3 - inB3;
|
||||||
|
*(pDst + 3) = inA4 - inB4;
|
||||||
|
|
||||||
|
|
||||||
|
/* Update pointers to process next sampels */
|
||||||
|
pSrcA += 4u;
|
||||||
|
pSrcB += 4u;
|
||||||
|
pDst += 4u;
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the results in the destination buffer. */
|
||||||
|
*pDst++ = (*pSrcA++) - (*pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicSub group
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sub_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 vector subtraction.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicSub
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q15 vector subtraction.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_sub_q15(
|
||||||
|
q15_t * pSrcA,
|
||||||
|
q15_t * pSrcB,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2;
|
||||||
|
q31_t inB1, inB2;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the results in the destination buffer two samples at a time. */
|
||||||
|
inA1 = *__SIMD32(pSrcA)++;
|
||||||
|
inA2 = *__SIMD32(pSrcA)++;
|
||||||
|
inB1 = *__SIMD32(pSrcB)++;
|
||||||
|
inB2 = *__SIMD32(pSrcB)++;
|
||||||
|
|
||||||
|
*__SIMD32(pDst)++ = __QSUB16(inA1, inB1);
|
||||||
|
*__SIMD32(pDst)++ = __QSUB16(inA2, inB2);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) __QSUB16(*pSrcA++, *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q15_t) __SSAT(((q31_t) * pSrcA++ - *pSrcB++), 16);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicSub group
|
||||||
|
*/
|
|
@ -0,0 +1,141 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sub_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 vector subtraction.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicSub
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q31 vector subtraction.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_sub_q31(
|
||||||
|
q31_t * pSrcA,
|
||||||
|
q31_t * pSrcB,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
q31_t inA1, inA2, inA3, inA4;
|
||||||
|
q31_t inB1, inB2, inB3, inB4;
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the results in the destination buffer. */
|
||||||
|
inA1 = *pSrcA++;
|
||||||
|
inA2 = *pSrcA++;
|
||||||
|
inB1 = *pSrcB++;
|
||||||
|
inB2 = *pSrcB++;
|
||||||
|
|
||||||
|
inA3 = *pSrcA++;
|
||||||
|
inA4 = *pSrcA++;
|
||||||
|
inB3 = *pSrcB++;
|
||||||
|
inB4 = *pSrcB++;
|
||||||
|
|
||||||
|
*pDst++ = __QSUB(inA1, inB1);
|
||||||
|
*pDst++ = __QSUB(inA2, inB2);
|
||||||
|
*pDst++ = __QSUB(inA3, inB3);
|
||||||
|
*pDst++ = __QSUB(inA4, inB4);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = __QSUB(*pSrcA++, *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ - *pSrcB++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicSub group
|
||||||
|
*/
|
|
@ -0,0 +1,126 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sub_q7.c
|
||||||
|
*
|
||||||
|
* Description: Q7 vector subtraction.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
|
||||||
|
*
|
||||||
|
* Version 1.1.0 2012/02/15
|
||||||
|
* Updated with more optimizations, bug fixes and minor API changes.
|
||||||
|
*
|
||||||
|
* Version 1.0.10 2011/7/15
|
||||||
|
* Big Endian support added and Merged M0 and M3/M4 Source code.
|
||||||
|
*
|
||||||
|
* Version 1.0.3 2010/11/29
|
||||||
|
* Re-organized the CMSIS folders and updated documentation.
|
||||||
|
*
|
||||||
|
* Version 1.0.2 2010/11/11
|
||||||
|
* Documentation updated.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2010/10/05
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 1.0.0 2010/09/20
|
||||||
|
* Production release and review comments incorporated.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BasicSub
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q7 vector subtraction.
|
||||||
|
* @param[in] *pSrcA points to the first input vector
|
||||||
|
* @param[in] *pSrcB points to the second input vector
|
||||||
|
* @param[out] *pDst points to the output vector
|
||||||
|
* @param[in] blockSize number of samples in each vector
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* The function uses saturating arithmetic.
|
||||||
|
* Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_sub_q7(
|
||||||
|
q7_t * pSrcA,
|
||||||
|
q7_t * pSrcB,
|
||||||
|
q7_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
uint32_t blkCnt; /* loop counter */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
/*loop Unrolling */
|
||||||
|
blkCnt = blockSize >> 2u;
|
||||||
|
|
||||||
|
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
|
||||||
|
** a second loop below computes the remaining 1 to 3 samples. */
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the results in the destination buffer 4 samples at a time. */
|
||||||
|
*__SIMD32(pDst)++ = __QSUB8(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
blkCnt = blockSize % 0x4u;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = __SSAT(*pSrcA++ - *pSrcB++, 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
/* Initialize blkCnt with number of samples */
|
||||||
|
blkCnt = blockSize;
|
||||||
|
|
||||||
|
while(blkCnt > 0u)
|
||||||
|
{
|
||||||
|
/* C = A - B */
|
||||||
|
/* Subtract and then store the result in the destination buffer. */
|
||||||
|
*pDst++ = (q7_t) __SSAT((q15_t) * pSrcA++ - *pSrcB++, 8);
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
blkCnt--;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BasicSub group
|
||||||
|
*/
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue