mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 04:46:51 +08:00
cleanup some unused/outdated stuff from in_progress
This commit is contained in:
Felix Ruess
@@ -1,40 +0,0 @@
|
|||||||
clear
|
|
||||||
close all
|
|
||||||
|
|
||||||
% DATA
|
|
||||||
t=1:200;
|
|
||||||
orig = sin(t./3)+sin(t./100.*2.*pi);
|
|
||||||
echo = [orig(4:end) 0 0 0]./1.2 + 0.5;
|
|
||||||
echo = echo + randn(size(orig))./10;
|
|
||||||
x = echo;
|
|
||||||
d = orig;
|
|
||||||
|
|
||||||
% RLS
|
|
||||||
rls_p = 5;
|
|
||||||
rls_delta = 1;
|
|
||||||
rls_lambda = 0.95;
|
|
||||||
rls_x = ones(rls_p+1, 1).* x(1);
|
|
||||||
rls_w = ones(rls_p+1, 1)./(rls_p+1);
|
|
||||||
rls_P = eye(rls_p+1) .* rls_delta;
|
|
||||||
|
|
||||||
filt = x;
|
|
||||||
|
|
||||||
for i=t
|
|
||||||
rls_x = [ x(i); rls_x(1:end-2); 1];
|
|
||||||
filt(i) = rls_w' * rls_x;
|
|
||||||
rls_alpha = d(i) - filt(i);
|
|
||||||
rls_g = rls_P * rls_x / (rls_lambda + rls_x' * rls_P * rls_x);
|
|
||||||
rls_P = rls_P / rls_lambda - rls_g*rls_x'* rls_P / rls_lambda;
|
|
||||||
rls_w = rls_w + rls_alpha * rls_g;
|
|
||||||
|
|
||||||
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
close all;
|
|
||||||
figure;
|
|
||||||
plot(orig,'r');
|
|
||||||
hold on;
|
|
||||||
plot(echo,'b');
|
|
||||||
plot(filt,'g');
|
|
||||||
|
|
||||||
@@ -1,99 +0,0 @@
|
|||||||
clear
|
|
||||||
close all
|
|
||||||
|
|
||||||
% DATA
|
|
||||||
tmax = 200;
|
|
||||||
t=1:tmax;
|
|
||||||
command = zeros(1,tmax);
|
|
||||||
command(50:150) = 1;
|
|
||||||
|
|
||||||
% reference model
|
|
||||||
vref = zeros(1,tmax);
|
|
||||||
vrefdot = 0;
|
|
||||||
|
|
||||||
sysx = 0.2;
|
|
||||||
sysvx = 0;
|
|
||||||
plotsys = zeros(1,tmax);
|
|
||||||
|
|
||||||
integrator = 0;
|
|
||||||
plotint =zeros(1,tmax);
|
|
||||||
|
|
||||||
x = zeros(1,1:tmax);
|
|
||||||
d = zeros(1,1:tmax);
|
|
||||||
filt = zeros(1,1:tmax);
|
|
||||||
|
|
||||||
% RLS
|
|
||||||
rls_p = 5;
|
|
||||||
rls_delta = 1;
|
|
||||||
rls_lambda = 0.95;
|
|
||||||
rls_x = ones(rls_p+1, 1).* x(1);
|
|
||||||
rls_w = ones(rls_p+1, 1)./(rls_p+1);
|
|
||||||
rls_P = eye(rls_p+1) .* rls_delta;
|
|
||||||
|
|
||||||
for i=2:tmax
|
|
||||||
acc = (command(i) - vref(i-1)) / 8 - vrefdot * 0.7 ;
|
|
||||||
if (acc > 0.02)
|
|
||||||
acc = 0.02;
|
|
||||||
elseif (acc<-0.02)
|
|
||||||
acc = -0.02;
|
|
||||||
end
|
|
||||||
vrefdot = vrefdot + acc;
|
|
||||||
if vrefdot > 0.5
|
|
||||||
vrefdot = 0.5
|
|
||||||
elseif vrefdot < -0.5
|
|
||||||
vrefdot = 0.5
|
|
||||||
end
|
|
||||||
vref(i) = vref(i-1) + vrefdot;
|
|
||||||
|
|
||||||
% Control with Integrator
|
|
||||||
error = vref(i) - sysx + rand(1,1) / 20.0;
|
|
||||||
integrator = integrator + error / 2;
|
|
||||||
syscomm = error * 4 + integrator;
|
|
||||||
|
|
||||||
sysacc = (syscomm - (sysx-0.2)*0.75) / 5 - sysvx*0.6;
|
|
||||||
if (sysacc > 0.04)
|
|
||||||
sysacc = 0.04;
|
|
||||||
elseif (sysacc<-0.04)
|
|
||||||
sysacc = -0.04;
|
|
||||||
end
|
|
||||||
sysvx = sysvx + sysacc;
|
|
||||||
if sysvx > 0.6
|
|
||||||
sysvx = 0.6
|
|
||||||
elseif sysvx < -0.7
|
|
||||||
sysvx = 0.7
|
|
||||||
end
|
|
||||||
sysx = sysx + sysvx;
|
|
||||||
|
|
||||||
plotsys(i) = sysx;
|
|
||||||
plotint(i) = integrator;
|
|
||||||
end
|
|
||||||
|
|
||||||
close all
|
|
||||||
figure
|
|
||||||
plot(command , 'r');
|
|
||||||
hold on
|
|
||||||
plot(vref,'g');
|
|
||||||
plot(plotsys,'b')
|
|
||||||
plot(plotint,'k');
|
|
||||||
|
|
||||||
%% Rest
|
|
||||||
|
|
||||||
|
|
||||||
for i=t
|
|
||||||
rls_x = [ x(i); rls_x(1:end-1)];
|
|
||||||
filt(i) = rls_w' * rls_x;
|
|
||||||
rls_alpha = d(i) - filt(i);
|
|
||||||
rls_g = rls_P * rls_x / (rls_lambda + rls_x' * rls_P * rls_x);
|
|
||||||
rls_P = rls_P / rls_lambda - rls_g*rls_x'* rls_P / rls_lambda;
|
|
||||||
rls_w = rls_w + rls_alpha * rls_g;
|
|
||||||
|
|
||||||
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
figure;
|
|
||||||
plot(orig,'r');
|
|
||||||
hold on;
|
|
||||||
plot(echo,'b');
|
|
||||||
plot(filt,'g');
|
|
||||||
|
|
||||||
@@ -1,429 +0,0 @@
|
|||||||
clear
|
|
||||||
clc
|
|
||||||
close all
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
dt = 1/60;
|
|
||||||
simsteps = 25000;
|
|
||||||
|
|
||||||
timespan=0:1/60:100;
|
|
||||||
|
|
||||||
t = 0;
|
|
||||||
airspeed = 14;
|
|
||||||
acceleration = 0;
|
|
||||||
vclimb = 0;
|
|
||||||
altitude = 100;
|
|
||||||
pitch = 0;
|
|
||||||
alpha = 0;
|
|
||||||
pitch_sp = 0.2;
|
|
||||||
throttle = 0.5;
|
|
||||||
power = 0.25;
|
|
||||||
elevator = 0;
|
|
||||||
|
|
||||||
|
|
||||||
% Altitude command
|
|
||||||
altitude_sp = ones(1,simsteps).* 100;
|
|
||||||
% Airspeed command
|
|
||||||
airspeed_sp = ones(1,simsteps).* 12;
|
|
||||||
|
|
||||||
% slow climb + descend
|
|
||||||
altitude_sp(1,2000:4000) = 125;
|
|
||||||
% fast climb + descend
|
|
||||||
airspeed_sp(1,6000:12000) = 16;
|
|
||||||
altitude_sp(1,8000:10000) = 125;
|
|
||||||
% too low airspeed
|
|
||||||
airspeed_sp(1,13000:14000) = 7;
|
|
||||||
% too high airspeed
|
|
||||||
airspeed_sp(1,15000:16000) = 30;
|
|
||||||
% throttle kill batlow
|
|
||||||
battery_good = ones(1,simsteps);
|
|
||||||
battery_good(1,24000:25000) = 0;
|
|
||||||
% Roll perturbation
|
|
||||||
roll_perturbation = zeros(1,simsteps);
|
|
||||||
% roll oscillation: e.g. poor nav
|
|
||||||
roll_perturbation(1,21000:24000) = abs(sin((21000:24000)./170 .* 6.28));
|
|
||||||
% Headwind changes by making turns
|
|
||||||
% -to keep a constant airspeed, a kinematic acceleration is needed
|
|
||||||
% -or with constant kinetic energy, a change in airspeed is seen
|
|
||||||
wind = zeros(1,simsteps);
|
|
||||||
% 60Hz 70m circle 11m/s 410m = ca 2000 samples
|
|
||||||
wind(1, 17000:23000) = sin((17000:23000)./2000 .* 6.28) .* 5;
|
|
||||||
% derivative of sin is cos so we might also just turn the wind 90deg
|
|
||||||
headwind_induced_kinematic_acceleration = wind .* 0.10;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Save states for plotting
|
|
||||||
X = zeros(simsteps,15);
|
|
||||||
extra1 = 0;
|
|
||||||
extra2 = 0;
|
|
||||||
|
|
||||||
% Very ultra-sofisticated Aircraft Model
|
|
||||||
Vmax = 26; % Level flight full power
|
|
||||||
Vmin = 8; % Stall
|
|
||||||
Mass = 1.0;
|
|
||||||
|
|
||||||
% Controller States
|
|
||||||
pitch_cumsum = 0;
|
|
||||||
airspeed_cumsum = 0;
|
|
||||||
|
|
||||||
% RLS
|
|
||||||
rls_p = 3;
|
|
||||||
rls_delta = 0;
|
|
||||||
rls_x = zeros(rls_p+1, 1);
|
|
||||||
rls_w = zeros(rls_p+1, 1);
|
|
||||||
rls_P = eye(rls_p+1) .* rls_delta;
|
|
||||||
|
|
||||||
|
|
||||||
for i=1:simsteps
|
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Simplified A/C long dynamics with stall and energy
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Pitch loop without integrators
|
|
||||||
pitch_neutral = 0.2;
|
|
||||||
elevator = (pitch_neutral + pitch_sp - pitch) * 15;
|
|
||||||
if (elevator > 0.4)
|
|
||||||
elevator = 0.4;
|
|
||||||
elseif (elevator < -0.2)
|
|
||||||
elevator = -0.2;
|
|
||||||
end
|
|
||||||
% Follow elevator but also align with the flow
|
|
||||||
pitch = pitch + elevator * dt - alpha * dt;
|
|
||||||
% Angle of attack from 0 deg at vmax to 17 deg stall at vmin
|
|
||||||
if airspeed > Vmax
|
|
||||||
alpha = 0;
|
|
||||||
LoverD = 6;
|
|
||||||
elseif airspeed < Vmin
|
|
||||||
% from Vmin downto 0.95 Vmin ... increase alpha from 17 to 45 deg
|
|
||||||
lin = (Vmin-airspeed) / (0.05*Vmin);
|
|
||||||
if (lin > 1)
|
|
||||||
lin = 1;
|
|
||||||
end
|
|
||||||
alpha = (17 + (60-17) * lin) / 57; % complete stall
|
|
||||||
LoverD = 8 - 7 * lin;
|
|
||||||
else
|
|
||||||
alpha = (Vmax - airspeed) / (Vmax-Vmin) * 17.0 / 57;
|
|
||||||
LoverD = 8;
|
|
||||||
end
|
|
||||||
% Path angle
|
|
||||||
gamma = pitch - alpha;
|
|
||||||
vclimb = sin(gamma) * airspeed;
|
|
||||||
altitude = altitude + vclimb * dt;
|
|
||||||
% Using the pitch angle...
|
|
||||||
P_climb = vclimb * Mass;
|
|
||||||
% Lift over Drag
|
|
||||||
P_drag = airspeed / (LoverD / (1+roll_perturbation(i))) * Mass;
|
|
||||||
% Motor Energy
|
|
||||||
power_sp = throttle ^ 2;
|
|
||||||
power = power + (power_sp - power) * 0.1;
|
|
||||||
% Full throttle = same energy as vmax glide
|
|
||||||
P_motor = (Vmax / LoverD) * sqrt(power) * Mass;
|
|
||||||
% Total energy
|
|
||||||
P_tot = P_motor - P_drag - P_climb;
|
|
||||||
|
|
||||||
acceleration = P_tot / Mass + headwind_induced_kinematic_acceleration(i);
|
|
||||||
airspeed = airspeed + (acceleration * dt);
|
|
||||||
|
|
||||||
if (airspeed < Vmin/2)
|
|
||||||
airspeed = Vmin/2;
|
|
||||||
end
|
|
||||||
if (altitude < 0)
|
|
||||||
altitude = 0;
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
controller = 4;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Run Control 1: Standard no-airspeed
|
|
||||||
|
|
||||||
if controller == 1
|
|
||||||
Throttle_min = 0.0;
|
|
||||||
Throttle_max = 1.;
|
|
||||||
|
|
||||||
Pitch_min = -30/57;
|
|
||||||
Pitch_max = 45/57;
|
|
||||||
|
|
||||||
VClimb_max = 3;
|
|
||||||
|
|
||||||
|
|
||||||
% Traditional with integrators:
|
|
||||||
|
|
||||||
vclimb_sp = (altitude_sp(i) - altitude);
|
|
||||||
if (vclimb_sp > VClimb_max)
|
|
||||||
vclimb_sp = VClimb_max;
|
|
||||||
elseif (vclimb_sp < -VClimb_max)
|
|
||||||
vclimb_sp = -VClimb_max;
|
|
||||||
end
|
|
||||||
% throttle increment only....
|
|
||||||
throttle = 0.65 + vclimb_sp * 0.9;
|
|
||||||
if (throttle > Throttle_max)
|
|
||||||
throttle = Throttle_max;
|
|
||||||
elseif (throttle < Throttle_min)
|
|
||||||
throttle = Throttle_min;
|
|
||||||
end
|
|
||||||
% pitch of vz
|
|
||||||
pitch_sp = 0.025 + vclimb_sp * 0.05;
|
|
||||||
if (pitch_sp > Pitch_max)
|
|
||||||
pitch_sp = Pitch_max;
|
|
||||||
elseif (pitch_sp < Pitch_min)
|
|
||||||
pitch_sp = Pitch_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Run Control 2-3: Old Airspeed Loops
|
|
||||||
% 2: with saturated integrator as most people used
|
|
||||||
% 3: with active integrator
|
|
||||||
|
|
||||||
elseif (controller == 2) || (controller == 3)
|
|
||||||
Throttle_min = 0.0;
|
|
||||||
Throttle_max = 1;
|
|
||||||
|
|
||||||
Pitch_min = -6/57;
|
|
||||||
Pitch_max = 15/57;
|
|
||||||
|
|
||||||
VClimb_max = 1.0;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
% Traditional with integrators:
|
|
||||||
|
|
||||||
vclimb_sp = (altitude_sp(i) - altitude);
|
|
||||||
if (vclimb_sp > VClimb_max)
|
|
||||||
vclimb_sp = VClimb_max;
|
|
||||||
elseif (vclimb_sp < -VClimb_max)
|
|
||||||
vclimb_sp = -VClimb_max;
|
|
||||||
end
|
|
||||||
% I-gain
|
|
||||||
pitch_cumsum = pitch_cumsum + (vclimb_sp - vclimb) * 0.0015;
|
|
||||||
if (pitch_cumsum > Pitch_max)
|
|
||||||
pitch_cumsum = Pitch_max;
|
|
||||||
elseif (pitch_cumsum < Pitch_min)
|
|
||||||
pitch_cumsum = Pitch_min;
|
|
||||||
end
|
|
||||||
% P-gain
|
|
||||||
pitch_sp = pitch_cumsum + vclimb_sp * 0.025;
|
|
||||||
if (pitch_sp > Pitch_max)
|
|
||||||
pitch_sp = Pitch_max;
|
|
||||||
elseif (pitch_sp < Pitch_min)
|
|
||||||
pitch_sp = Pitch_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
err_airspeed = airspeed_sp(i) - airspeed;
|
|
||||||
if (controller == 2)
|
|
||||||
airspeed_cumsum = 0.45;
|
|
||||||
else
|
|
||||||
airspeed_cumsum = airspeed_cumsum + err_airspeed * 0.001;
|
|
||||||
end
|
|
||||||
if (airspeed_cumsum > Throttle_max)
|
|
||||||
airspeed_cumsum = Throttle_max;
|
|
||||||
elseif (airspeed_cumsum < Throttle_min)
|
|
||||||
airspeed_cumsum = Throttle_min;
|
|
||||||
end
|
|
||||||
throttle = airspeed_cumsum + err_airspeed * 0.2;
|
|
||||||
if (throttle > Throttle_max)
|
|
||||||
throttle = Throttle_max;
|
|
||||||
elseif (throttle < Throttle_min)
|
|
||||||
throttle = Throttle_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Run Control 4: Feed Forward Gains to climb/accelerate Tcr+Tcl
|
|
||||||
|
|
||||||
elseif controller == 4
|
|
||||||
Throttle_min = 0.0;
|
|
||||||
Throttle_max = 1;
|
|
||||||
|
|
||||||
Pitch_min = -12/57;
|
|
||||||
Pitch_max = 25/57;
|
|
||||||
|
|
||||||
if (airspeed_sp(i) <= 12)
|
|
||||||
VClimb_max = 3;
|
|
||||||
Pitch_max = 10.5/57;
|
|
||||||
Pitch_min = -4.5/57;
|
|
||||||
else
|
|
||||||
VClimb_max = 2;
|
|
||||||
Pitch_max = 3.5/57;
|
|
||||||
Pitch_min = -11/57;
|
|
||||||
end
|
|
||||||
|
|
||||||
% Climb feed forward gain per m/s
|
|
||||||
Tcl = 0.25;
|
|
||||||
% Cruise feed forward gain per m/s
|
|
||||||
Tcr = 1/26;
|
|
||||||
|
|
||||||
% Traditional with integrators:
|
|
||||||
|
|
||||||
vclimb_sp = (altitude_sp(i) - altitude);
|
|
||||||
if (vclimb_sp > VClimb_max)
|
|
||||||
vclimb_sp = VClimb_max;
|
|
||||||
elseif (vclimb_sp < -VClimb_max)
|
|
||||||
vclimb_sp = -VClimb_max;
|
|
||||||
end
|
|
||||||
% I-gain
|
|
||||||
pitch_cumsum = pitch_cumsum + (vclimb_sp - vclimb) * 0.0015;
|
|
||||||
if (pitch_cumsum > Pitch_max)
|
|
||||||
pitch_cumsum = Pitch_max;
|
|
||||||
elseif (pitch_cumsum < Pitch_min)
|
|
||||||
pitch_cumsum = Pitch_min;
|
|
||||||
end
|
|
||||||
% P-gain
|
|
||||||
pitch_sp = pitch_cumsum + vclimb_sp * 0.025;
|
|
||||||
if (pitch_sp > Pitch_max)
|
|
||||||
pitch_sp = Pitch_max;
|
|
||||||
elseif (pitch_sp < Pitch_min)
|
|
||||||
pitch_sp = Pitch_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
err_airspeed = airspeed_sp(i) - airspeed;
|
|
||||||
airspeed_cumsum = airspeed_cumsum + err_airspeed * 0.0005;
|
|
||||||
if (airspeed_cumsum > Throttle_max)
|
|
||||||
airspeed_cumsum = Throttle_max;
|
|
||||||
elseif (airspeed_cumsum < Throttle_min)
|
|
||||||
airspeed_cumsum = Throttle_min;
|
|
||||||
end
|
|
||||||
throttle = airspeed_cumsum + err_airspeed * 0.2 + Tcr * airspeed_sp(i) + Tcl * vclimb_sp;
|
|
||||||
if (throttle > Throttle_max)
|
|
||||||
throttle = Throttle_max;
|
|
||||||
elseif (throttle < Throttle_min)
|
|
||||||
throttle = Throttle_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
||||||
% Run Control 5: Feed Forward Gains to climb/accelerate Tcr+Tcl
|
|
||||||
|
|
||||||
elseif controller == 5
|
|
||||||
Throttle_min = 0.0;
|
|
||||||
Throttle_max = 1;
|
|
||||||
|
|
||||||
Pitch_min = -6/57;
|
|
||||||
Pitch_max = 15/57;
|
|
||||||
|
|
||||||
VClimb_max = 1.0;
|
|
||||||
|
|
||||||
if (i==1)
|
|
||||||
% Climb feed forward gain per m/s
|
|
||||||
Tcl = 0.25;
|
|
||||||
% Cruise feed forward gain per m/s
|
|
||||||
Tcr = 1/2000;
|
|
||||||
end
|
|
||||||
|
|
||||||
% Traditional with integrators:
|
|
||||||
|
|
||||||
vclimb_sp = (altitude_sp(i) - altitude);
|
|
||||||
if (vclimb_sp > VClimb_max)
|
|
||||||
vclimb_sp = VClimb_max;
|
|
||||||
elseif (vclimb_sp < -VClimb_max)
|
|
||||||
vclimb_sp = -VClimb_max;
|
|
||||||
end
|
|
||||||
% I-gain
|
|
||||||
pitch_cumsum = pitch_cumsum + (vclimb_sp - vclimb) * 0.0015;
|
|
||||||
if (pitch_cumsum > Pitch_max)
|
|
||||||
pitch_cumsum = Pitch_max;
|
|
||||||
elseif (pitch_cumsum < Pitch_min)
|
|
||||||
pitch_cumsum = Pitch_min;
|
|
||||||
end
|
|
||||||
% P-gain
|
|
||||||
pitch_sp = pitch_cumsum + vclimb_sp * 0.025;
|
|
||||||
if (pitch_sp > Pitch_max)
|
|
||||||
pitch_sp = Pitch_max;
|
|
||||||
elseif (pitch_sp < Pitch_min)
|
|
||||||
pitch_sp = Pitch_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
err_airspeed = airspeed_sp(i) - airspeed;
|
|
||||||
if (abs(vclimb_sp) > 0.5)
|
|
||||||
%Tcl = Tcl + err_airspeed/airspeed_sp(i) * 0.05 * 0.01;
|
|
||||||
Tcr = Tcr + err_airspeed/airspeed_sp(i) * 0.05 * 0.001;
|
|
||||||
else
|
|
||||||
%Tcl = Tcl + err_airspeed/airspeed_sp(i) * 0.05 * 0.001;
|
|
||||||
Tcr = Tcr + err_airspeed/airspeed_sp(i) * 0.05 * 0.01;
|
|
||||||
end
|
|
||||||
throttle = err_airspeed * 0.2 + Tcr * airspeed_sp(i) + Tcl * vclimb_sp;
|
|
||||||
if (throttle > Throttle_max)
|
|
||||||
throttle = Throttle_max;
|
|
||||||
elseif (throttle < Throttle_min)
|
|
||||||
throttle = Throttle_min;
|
|
||||||
end
|
|
||||||
|
|
||||||
extra1 = Tcr;
|
|
||||||
end
|
|
||||||
|
|
||||||
% Autopilot actions
|
|
||||||
if battery_good(i) == 0
|
|
||||||
throttle = 0;
|
|
||||||
end
|
|
||||||
|
|
||||||
% Save For plotting
|
|
||||||
X(i,:) = [ airspeed_sp(i) airspeed altitude_sp(i) altitude throttle power pitch_sp (pitch-pitch_neutral) alpha gamma elevator vclimb_sp vclimb extra1 extra2];
|
|
||||||
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
figure
|
|
||||||
subplot(6,1,1)
|
|
||||||
hold on
|
|
||||||
plot(airspeed_sp,'r');
|
|
||||||
plot(X(:,2),'b');
|
|
||||||
plot(ones(simsteps,1)*Vmin,'k');
|
|
||||||
plot(ones(simsteps,1)*Vmax,'k');
|
|
||||||
title('Airspeed')
|
|
||||||
axis([0 simsteps Vmin-1 Vmax+1]);
|
|
||||||
grid
|
|
||||||
|
|
||||||
subplot(6,1,2)
|
|
||||||
hold on
|
|
||||||
plot(altitude_sp,'r');
|
|
||||||
plot(X(:,4),'b');
|
|
||||||
title('Altitude')
|
|
||||||
axis([0 simsteps (min(altitude_sp) - 10) (max(altitude_sp)+10)]);
|
|
||||||
grid
|
|
||||||
|
|
||||||
subplot(6,1,3)
|
|
||||||
hold on
|
|
||||||
plot(X(:,5),'r');
|
|
||||||
title('Throttle')
|
|
||||||
axis([0 simsteps 0 1]);
|
|
||||||
grid
|
|
||||||
|
|
||||||
|
|
||||||
subplot(6,1,4)
|
|
||||||
hold on
|
|
||||||
plot(X(:,7)*57,'r');
|
|
||||||
plot(X(:,8)*57,'b');
|
|
||||||
title('Pitch (degrees)')
|
|
||||||
axis([0 simsteps (Pitch_min*57-3) (Pitch_max*57 + 3)]);
|
|
||||||
grid
|
|
||||||
|
|
||||||
subplot(6,1,5)
|
|
||||||
hold on
|
|
||||||
plot(X(:,9)*57,'g');
|
|
||||||
plot(ones(simsteps,1)*17,'r');
|
|
||||||
plot(X(:,10)*57,'k');
|
|
||||||
axis([0 simsteps -10 20]);
|
|
||||||
title('Alpha (green) Gamma (black) (degrees)')
|
|
||||||
%legend('1','2')
|
|
||||||
grid
|
|
||||||
|
|
||||||
subplot(6,1,6)
|
|
||||||
hold on
|
|
||||||
plot(X(:,12),'r');
|
|
||||||
plot(X(:,13),'b');
|
|
||||||
title('VClimb')
|
|
||||||
axis([0 simsteps (-VClimb_max -0.5) (VClimb_max + 0.5)]);
|
|
||||||
grid
|
|
||||||
|
|
||||||
set(gcf,'PaperUnits','centimeter')
|
|
||||||
set(gcf,'PaperSize',[21 30.5])
|
|
||||||
set(gcf,'PaperPosition',[1 1 19 28.5])
|
|
||||||
print(gcf,'-depsc2', ['results_' num2str(controller)]);
|
|
||||||
%print(gcf,'-djpeg', ['results_' num2str(controller)]);
|
|
||||||
|
|
||||||
|
|
||||||
@@ -1,109 +0,0 @@
|
|||||||
//
|
|
||||||
// State X = [z; a]
|
|
||||||
// X(k+1) = X(k) + [ a * delta_p; 0]
|
|
||||||
//
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
clear();
|
|
||||||
getf('baro_utils.sci');
|
|
||||||
|
|
||||||
//filename = "data/07_08_02__14_57_30.data";
|
|
||||||
//filename = "data/07_08_02__15_19_47.data";
|
|
||||||
//filename = "data/07_09_09__16_23_58.data";
|
|
||||||
filename = "data/test.data";
|
|
||||||
|
|
||||||
[time_pressure, pressure, time_altitude, altitude] = baro_read_pprz_log(filename);
|
|
||||||
|
|
||||||
//
|
|
||||||
// Initialisation
|
|
||||||
//
|
|
||||||
time_start = 110.;
|
|
||||||
time_end = 250.;
|
|
||||||
//time_start = 100.;
|
|
||||||
//time_end = 150.;
|
|
||||||
|
|
||||||
[pressure0,end_pressure, altitude0, end_altitude, a0, b0] = filter_init_timed(time_start, time_end, time_pressure, pressure, time_altitude, altitude)
|
|
||||||
|
|
||||||
X0 = [ altitude0; a0];
|
|
||||||
|
|
||||||
P0 = [ 1. 0.
|
|
||||||
0. 2. ];
|
|
||||||
|
|
||||||
X = [X0];
|
|
||||||
P = [P0];
|
|
||||||
time_state = [time_pressure(end_pressure)];
|
|
||||||
//
|
|
||||||
// Iterations
|
|
||||||
//
|
|
||||||
idx_a = end_altitude;
|
|
||||||
idx_s = 2;
|
|
||||||
for idx_p=(end_pressure+1):length(time_pressure)
|
|
||||||
|
|
||||||
// prediction
|
|
||||||
// initial state
|
|
||||||
X0 = X(:, idx_s-1);
|
|
||||||
// initial covariance
|
|
||||||
P0 = baro_get_P(P, idx_s-1);
|
|
||||||
// command
|
|
||||||
delta_p = pressure(idx_p) - pressure(idx_p-1);
|
|
||||||
delta_z = X0(2) * delta_p;
|
|
||||||
X1 = X0 + [delta_z; 0];
|
|
||||||
// jacobian
|
|
||||||
F = [ 1. delta_p
|
|
||||||
0 1. ];
|
|
||||||
// process covariance noise
|
|
||||||
Q = [ 1e-4 0.
|
|
||||||
0. 1e-7 ];
|
|
||||||
P1 = F*P0*F' + Q;
|
|
||||||
|
|
||||||
// update
|
|
||||||
err = altitude(idx_a) - X1(1);
|
|
||||||
H = [1 0];
|
|
||||||
R = 25;
|
|
||||||
E = H * P1 * H' + R;
|
|
||||||
K = P1 * H' * inv(E);
|
|
||||||
P2 = P1 - K * H * P1;
|
|
||||||
X2 = X1 + K * err;
|
|
||||||
|
|
||||||
X = [X X2];
|
|
||||||
P = [P P2];
|
|
||||||
time_state = [time_state time_pressure(idx_p)];
|
|
||||||
idx_p = idx_p + 1;
|
|
||||||
idx_s = idx_s + 1;
|
|
||||||
while ((idx_p < length(time_pressure)) & ...
|
|
||||||
(time_altitude(idx_a) < time_pressure(idx_p)) & ...
|
|
||||||
(idx_a < length(time_altitude))), idx_a = idx_a + 1; end
|
|
||||||
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
dumb_alt = a0 * pressure + b0;
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// Display
|
|
||||||
//
|
|
||||||
xbasc();
|
|
||||||
subplot(4,1,1)
|
|
||||||
xtitle('altitude');
|
|
||||||
plot2d([time_altitude]', [altitude]', style=[5]);
|
|
||||||
plot2d([time_state]', [X(1,:)]', style=[3, 5], leg="est_alt@gps");
|
|
||||||
plot2d([time_pressure]', [dumb_alt]', style=[1]);
|
|
||||||
subplot(4,1,2)
|
|
||||||
xtitle('pressure');
|
|
||||||
plot2d([time_pressure]', [pressure]', style=[5], leg="pressure");
|
|
||||||
subplot(4,1,3)
|
|
||||||
xtitle('a');
|
|
||||||
plot2d([time_state]', [X(2,:)]', style=[3], leg="a");
|
|
||||||
subplot(4,1,4)
|
|
||||||
xtitle('covariance');
|
|
||||||
P11 = [];
|
|
||||||
P22 = [];
|
|
||||||
for i=1:length(time_state)
|
|
||||||
Pi = baro_get_P(P, i);
|
|
||||||
P11 = [P11 Pi(1,1)];
|
|
||||||
P22 = [P22 Pi(2,2)];
|
|
||||||
end
|
|
||||||
plot2d([time_state; time_state]', [P11; P22]', style=[5 3], leg="Palt@Pa");
|
|
||||||
@@ -1,202 +0,0 @@
|
|||||||
function [time, gps_alt, pressure, gps_climb, temp] = baro_read_log(filename)
|
|
||||||
|
|
||||||
time=[];
|
|
||||||
gps_alt=[];
|
|
||||||
pressure=[];
|
|
||||||
gps_climb=[];
|
|
||||||
temp=[];
|
|
||||||
|
|
||||||
u=mopen(filename, 'r');
|
|
||||||
idx = 0;
|
|
||||||
while meof(u) == 0,
|
|
||||||
line = mgetl(u, 1);
|
|
||||||
[nb_scan, ti, ga, p, gc, te] = msscanf(1, line, '%f %f %f %f %f');
|
|
||||||
time = [time ti];
|
|
||||||
gps_alt = [gps_alt ga];
|
|
||||||
pressure = [pressure p];
|
|
||||||
gps_climb = [gps_climb gc];
|
|
||||||
temp = [temp te];
|
|
||||||
end
|
|
||||||
mclose(u);
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
function [time_pressure, pressure, time_gps, altitude, time_accel, ax_accel, az_accel] = baro_read_pprz_log(filename)
|
|
||||||
|
|
||||||
time_pressure= [];
|
|
||||||
pressure = [];
|
|
||||||
time_gps = [];
|
|
||||||
altitude = [];
|
|
||||||
time_accel = [];
|
|
||||||
ax_accel = [];
|
|
||||||
az_accel = [];
|
|
||||||
|
|
||||||
no_line = 0;
|
|
||||||
u=mopen(filename, 'r');
|
|
||||||
while meof(u) == 0 & no_line < 30000,
|
|
||||||
line = mgetl(u, 1);
|
|
||||||
if (line == "") continue end
|
|
||||||
// printf('%s\n', line);
|
|
||||||
[nb_scan, tip, ac, pr, te, direct_alt] = msscanf(1, line, '%f %d BARO_MS5534A %d %d %f');
|
|
||||||
if (nb_scan == 5)
|
|
||||||
time_pressure = [time_pressure tip];
|
|
||||||
pressure = [pressure pr];
|
|
||||||
else
|
|
||||||
[nb_scan, tig, ac, gmod, gue, gun, gcour, galt, gspeed, gclimb, gitow, guzone, gnerr] = ...
|
|
||||||
msscanf(1, line, '%f %d GPS %d %d %d %d %d %d %d %d %d %d');
|
|
||||||
if (nb_scan == 12)
|
|
||||||
time_gps = [time_gps tig];
|
|
||||||
altitude = [altitude galt];
|
|
||||||
else
|
|
||||||
[nb_scan, tiacc, ac, ax, az] = ...
|
|
||||||
msscanf(1, line, '%f %d ADC_GENERIC %d %d');
|
|
||||||
if (nb_scan == 4)
|
|
||||||
time_accel = [time_accel tiacc];
|
|
||||||
ax_accel = [ax_accel ax];
|
|
||||||
az_accel = [az_accel az];
|
|
||||||
end
|
|
||||||
end
|
|
||||||
end
|
|
||||||
no_line = no_line + 1;
|
|
||||||
end
|
|
||||||
mclose(u);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
function [baro_alt] = compute_altitude_exp(baro_pressure)
|
|
||||||
P0 = 1013.25;
|
|
||||||
T0 = 288.15;
|
|
||||||
Tg =6.5/1000;
|
|
||||||
R = 287.052;
|
|
||||||
g = 9.81;
|
|
||||||
baro_alt=[];
|
|
||||||
len = length(baro_pressure)
|
|
||||||
for i=1:len
|
|
||||||
ba = T0/Tg*(1-(baro_pressure(i)/P0)^(Tg*R/g));
|
|
||||||
baro_alt=[baro_alt ba];
|
|
||||||
end
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
function [baro_alt] = compute_altitude_lin(baro_pressure)
|
|
||||||
baro_alt=[];
|
|
||||||
len = length(baro_pressure)
|
|
||||||
for idx=1:len
|
|
||||||
p = 10 * baro_pressure(idx);
|
|
||||||
if (p > 10300)
|
|
||||||
i = 1638.;
|
|
||||||
j = -139.;
|
|
||||||
pl = 10300.;
|
|
||||||
elseif ( p > 9700 )
|
|
||||||
i = 1720.;
|
|
||||||
j = 365.;
|
|
||||||
pl = 9700.;
|
|
||||||
elseif ( p > 9200 )
|
|
||||||
i = 1802.;
|
|
||||||
j = 805.;
|
|
||||||
pl = 9200.;
|
|
||||||
elseif ( p > 8500 )
|
|
||||||
i = 1905.;
|
|
||||||
j = 1456.;
|
|
||||||
pl = 8500.;
|
|
||||||
elseif ( p > 7800 )
|
|
||||||
|
|
||||||
elseif ( p > 7100 )
|
|
||||||
|
|
||||||
end
|
|
||||||
ba = j - (p-pl) * i / 2^11;
|
|
||||||
baro_alt = [baro_alt ba];
|
|
||||||
end
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// intersema application note 501 page 8
|
|
||||||
//
|
|
||||||
function [pres, alt, a, b] = filter_init(avg_len, pressure, gps_alt)
|
|
||||||
|
|
||||||
avg_pressure = sum(pressure(1:avg_len), 'c') / avg_len;
|
|
||||||
avg_gps = sum(gps_alt(1:avg_len), 'c') / avg_len;
|
|
||||||
|
|
||||||
if (avg_pressure > 1030.0)
|
|
||||||
a = -16380. / 2^11;
|
|
||||||
elseif (avg_pressure > 970.0)
|
|
||||||
a = -17200. / 2^11;
|
|
||||||
elseif (avg_pressure > 920.0)
|
|
||||||
a = -18020. / 2^11;
|
|
||||||
elseif (avg_pressure > 850.0)
|
|
||||||
a = -19050. / 2^11;
|
|
||||||
elseif (avg_pressure > 780.0)
|
|
||||||
a = -20330. / 2^11;
|
|
||||||
elseif (avg_pressure > 710.0)
|
|
||||||
a = -21880. / 2^11;
|
|
||||||
elseif (avg_pressure > 650.0)
|
|
||||||
a = -23590. / 2^11;
|
|
||||||
end;
|
|
||||||
|
|
||||||
pres = avg_pressure;
|
|
||||||
alt = avg_gps;
|
|
||||||
b = avg_gps - a * avg_pressure;
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
function [pressure0,end_pressure, altitude0, end_altitude, a0, b0] = filter_init_timed(time_start, time_end, time_pressure, pressure, time_altitude, altitude)
|
|
||||||
[avg_pressure, start_pressure, end_pressure] = average_period(time_start, time_end, time_pressure, pressure);
|
|
||||||
[avg_altitude, start_altitude, end_altitude] = average_period(time_start, time_end, time_altitude, altitude);
|
|
||||||
|
|
||||||
if (avg_pressure > 1030.0)
|
|
||||||
a0 = -16380. / 2^11;
|
|
||||||
elseif (avg_pressure > 970.0)
|
|
||||||
a0 = -17200. / 2^11;
|
|
||||||
elseif (avg_pressure > 920.0)
|
|
||||||
a0 = -18020. / 2^11;
|
|
||||||
elseif (avg_pressure > 850.0)
|
|
||||||
a0 = -19050. / 2^11;
|
|
||||||
elseif (avg_pressure > 780.0)
|
|
||||||
a0 = -20330. / 2^11;
|
|
||||||
elseif (avg_pressure > 710.0)
|
|
||||||
a0 = -21880. / 2^11;
|
|
||||||
elseif (avg_pressure > 650.0)
|
|
||||||
a0 = -23590. / 2^11;
|
|
||||||
end;
|
|
||||||
|
|
||||||
pressure0 = avg_pressure;
|
|
||||||
altitude0 = avg_altitude;
|
|
||||||
b0 = avg_altitude - a0 * avg_pressure;
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
function [average, idx_start, idx_end] = average_period(time_start, time_end, time_samples, samples)
|
|
||||||
idx_start = 1;
|
|
||||||
while ((time_samples(idx_start) < time_start) & (idx_start <= length(time_samples))),
|
|
||||||
idx_start = idx_start+1;
|
|
||||||
end
|
|
||||||
idx_end = idx_start;
|
|
||||||
while ((time_samples(idx_end) < time_end) & (idx_end <= length(time_samples))),
|
|
||||||
idx_end = idx_end+1;
|
|
||||||
end
|
|
||||||
average = sum(samples(idx_start:idx_end), 'c') / (idx_end - idx_start);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
function [Pi] = baro_get_P(P, i)
|
|
||||||
|
|
||||||
Pi = P(:, 2*i-1:2*i);
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
function [Pi] = baro_get_P3(P, i)
|
|
||||||
|
|
||||||
Pi = P(:, 3*i-2:3*i);
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
@@ -1,50 +0,0 @@
|
|||||||
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
function [X1, P1] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q)
|
|
||||||
|
|
||||||
X1 = X0 + X0dot * dt;
|
|
||||||
P0dot = F*P0 + P0*F' + Q;
|
|
||||||
P1 = P0 + P0dot * dt;
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q)
|
|
||||||
|
|
||||||
X1 = X0 + X0dot * dt;
|
|
||||||
expF = expm(dt * F);
|
|
||||||
P0dot = expF*P0*expF' + Q;
|
|
||||||
P1 = P0 + P0dot * dt;
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
function [X1, P1] = ekf_update(X0, P0, H, R, err)
|
|
||||||
|
|
||||||
E = H * P0 * H' + R;
|
|
||||||
K = P0 * H' * inv(E);
|
|
||||||
P1 = P0 - K * H * P0;
|
|
||||||
X1 = X0 + K * err;
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -1,38 +0,0 @@
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
clear();
|
|
||||||
getf('baro_utils.sci');
|
|
||||||
|
|
||||||
filename = "data/07_07_26__16_37_09.baro.txt";
|
|
||||||
[time, gps_alt, pressure, gps_climb, temp] = baro_read_log(filename);
|
|
||||||
|
|
||||||
|
|
||||||
[p0, z0, a, b] = filter_init(10, pressure, gps_alt)
|
|
||||||
|
|
||||||
if 0
|
|
||||||
deff('e=G(p,z)','a=p(1),b=p(2), gps=z(1), pressure=z(2), e=gps-(a/pressure+b)')
|
|
||||||
deff('dg=dG(p,z)','a=p(1),b=p(2), gps=z(1), pressure=z(2), dg = [-1/pressure; -b]')
|
|
||||||
p0 = [a, 0]';
|
|
||||||
[p, err] = fit_dat(G, p0, [gps_alt; pressure], [1], dG)
|
|
||||||
|
|
||||||
a = p(1)
|
|
||||||
b = p(2)
|
|
||||||
end
|
|
||||||
|
|
||||||
baro_alt_lin = a * pressure + b;
|
|
||||||
baro_alt_full = compute_altitude_exp(pressure);
|
|
||||||
//baro_alt = compute_altitude_lin(pressure);
|
|
||||||
|
|
||||||
xbasc();
|
|
||||||
if 0
|
|
||||||
subplot(3,1,1)
|
|
||||||
xtitle('Gps');
|
|
||||||
plot2d([time], [gps_alt]);
|
|
||||||
subplot(3,1,2)
|
|
||||||
xtitle('Pressure');
|
|
||||||
plot2d([time], [pressure]);
|
|
||||||
subplot(3,1,3)
|
|
||||||
end
|
|
||||||
xtitle('Both');
|
|
||||||
plot2d([time; time; time]', [gps_alt; baro_alt_full; baro_alt_lin]', style=[5, 3, 2], leg="gps@barofull@barolin");
|
|
||||||
@@ -1,141 +0,0 @@
|
|||||||
//
|
|
||||||
// State X = [z; a]
|
|
||||||
// X(k+1) = X(k) + [ a * delta_p; 0]
|
|
||||||
//
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
clear();
|
|
||||||
getf('baro_utils.sci');
|
|
||||||
|
|
||||||
//filename = "data/07_08_02__14_57_30.data";
|
|
||||||
//filename = "data/07_08_02__15_19_47.data";
|
|
||||||
//filename = "data/07_09_09__16_23_58.data";
|
|
||||||
filename = "data/test.data";
|
|
||||||
|
|
||||||
[time_pressure, pressure, time_altitude, altitude, time_accel, accel_x, accel_z] = baro_read_pprz_log(filename);
|
|
||||||
|
|
||||||
acc_neutral = 512;
|
|
||||||
acc_gain = 2. / 400 * 9.81;
|
|
||||||
|
|
||||||
ax_ms2 = (accel_x - acc_neutral) * acc_gain;
|
|
||||||
az_ms2 = (accel_z - acc_neutral) * acc_gain;
|
|
||||||
|
|
||||||
norm_accel = sqrt( ax_ms2^2 + az_ms2^2);
|
|
||||||
meas_pitch = atan(-az_ms2, ax_ms2);
|
|
||||||
z_dot = [0.]
|
|
||||||
|
|
||||||
//
|
|
||||||
// Initialisation
|
|
||||||
//
|
|
||||||
time_start = 110.;
|
|
||||||
time_end = 250.;
|
|
||||||
//time_start = 100.;
|
|
||||||
//time_end = 150.;
|
|
||||||
|
|
||||||
[pressure0,end_pressure, altitude0, end_altitude, a0, b0] = filter_init_timed(time_start, time_end, time_pressure, pressure, time_altitude, altitude)
|
|
||||||
|
|
||||||
zdot_0 = 0.;
|
|
||||||
|
|
||||||
X0 = [ altitude0; zdot_0];
|
|
||||||
|
|
||||||
P0 = [ 1. 0.
|
|
||||||
0. 2. ];
|
|
||||||
|
|
||||||
X = [X0];
|
|
||||||
P = [P0];
|
|
||||||
time_state = [time_pressure(end_pressure)];
|
|
||||||
//
|
|
||||||
// Iterations
|
|
||||||
//
|
|
||||||
idx_a = end_altitude;
|
|
||||||
idx_acc = 1;
|
|
||||||
while idx_acc < length(time_accel) & time_accel(idx_acc) < time_pressure(end_pressure), idx_acc = idx_acc + 1; end
|
|
||||||
idx_s = 2;
|
|
||||||
for idx_p=(end_pressure+1):length(time_pressure)
|
|
||||||
|
|
||||||
// prediction
|
|
||||||
// initial state
|
|
||||||
X0 = X(:, idx_s-1);
|
|
||||||
// initial covariance
|
|
||||||
P0 = baro_get_P(P, idx_s-1);
|
|
||||||
// command
|
|
||||||
delta_p = pressure(idx_p) - pressure(idx_p-1);
|
|
||||||
delta_z = a0 * delta_p;
|
|
||||||
X1 = X0 + [delta_z; 0];
|
|
||||||
// jacobian
|
|
||||||
F = [ 1. delta_p
|
|
||||||
0 1. ];
|
|
||||||
// process covariance noise
|
|
||||||
Q = [ 1e-4 0.
|
|
||||||
0. 1e-7 ];
|
|
||||||
P1 = F*P0*F' + Q;
|
|
||||||
|
|
||||||
// update
|
|
||||||
z_dot_m = (norm_accel(idx_acc) - 9.81) * (time_accel(idx_acc) - time_accel(idx_acc - 1)) + X(2, idx_p-end_pressure);
|
|
||||||
z_dot = [z_dot z_dot_m];
|
|
||||||
err = z_dot_m - X1(2);
|
|
||||||
H = [0 1];
|
|
||||||
R = 25;
|
|
||||||
E = H * P1 * H' + R;
|
|
||||||
K = P1 * H' * inv(E);
|
|
||||||
P2 = P1 - K * H * P1;
|
|
||||||
X2 = X1 + K * err;
|
|
||||||
|
|
||||||
X = [X X2];
|
|
||||||
P = [P P2];
|
|
||||||
time_state = [time_state time_pressure(idx_p)];
|
|
||||||
idx_p = idx_p + 1;
|
|
||||||
idx_s = idx_s + 1;
|
|
||||||
while ((idx_p < length(time_pressure)) & ...
|
|
||||||
(time_altitude(idx_a) < time_pressure(idx_p)) & ...
|
|
||||||
(idx_a < length(time_altitude))), idx_a = idx_a + 1; end
|
|
||||||
|
|
||||||
while ((idx_p < length(time_pressure)) & ...
|
|
||||||
(time_accel(idx_acc) < time_pressure(idx_p)) & ...
|
|
||||||
(idx_acc < length(time_accel))), idx_acc = idx_acc + 1; end
|
|
||||||
|
|
||||||
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
dumb_alt = a0 * pressure + b0;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// Display
|
|
||||||
//
|
|
||||||
xbasc();
|
|
||||||
subplot(6,1,1)
|
|
||||||
xtitle('altitude');
|
|
||||||
plot2d([time_altitude]', [altitude]', style=[5]);
|
|
||||||
plot2d([time_state]', [X(1,:)]', style=[3, 5], leg="est_alt@gps");
|
|
||||||
plot2d([time_pressure]', [dumb_alt]', style=[1]);
|
|
||||||
subplot(6,1,2)
|
|
||||||
xtitle('pressure');
|
|
||||||
plot2d([time_pressure]', [pressure]', style=[5], leg="pressure");
|
|
||||||
subplot(6,1,3)
|
|
||||||
xtitle('z_dot');
|
|
||||||
plot2d([time_state]', [X(2,:)]', style=[3], leg="zdot_est");
|
|
||||||
plot2d([time_state]', [z_dot]', style=[3], leg="zdot_meas");
|
|
||||||
subplot(6,1,4)
|
|
||||||
xtitle('covariance');
|
|
||||||
P11 = [];
|
|
||||||
P22 = [];
|
|
||||||
for i=1:length(time_state)
|
|
||||||
Pi = baro_get_P(P, i);
|
|
||||||
P11 = [P11 Pi(1,1)];
|
|
||||||
P22 = [P22 Pi(2,2)];
|
|
||||||
end
|
|
||||||
plot2d([time_state; time_state]', [P11; P22]', style=[5 3], leg="Pzt@Pzdot");
|
|
||||||
|
|
||||||
|
|
||||||
subplot(6,1,5)
|
|
||||||
plot2d([time_accel; time_accel; time_accel]', [ax_ms2; az_ms2; norm_accel]', style=[5 3 2], leg="Ax@Az@anorm");
|
|
||||||
|
|
||||||
|
|
||||||
subplot(6,1,6)
|
|
||||||
plot2d([time_accel]', [meas_pitch]', style=[5 3 2], leg="Ax@Az@anorm");
|
|
||||||
@@ -1,22 +0,0 @@
|
|||||||
<conf>
|
|
||||||
<aircraft
|
|
||||||
name="R1"
|
|
||||||
ac_id="26"
|
|
||||||
airframe="airframes/red_one.xml"
|
|
||||||
radio="radios/mc24.xml"
|
|
||||||
telemetry="telemetry/default.xml"
|
|
||||||
flight_plan="flight_plans/ccc_gl.xml"
|
|
||||||
settings="settings/cam.xml settings/tuning.xml settings/switch.xml"
|
|
||||||
gui_color="yellow"
|
|
||||||
/>
|
|
||||||
<aircraft
|
|
||||||
name="TJ1"
|
|
||||||
ac_id="6"
|
|
||||||
airframe="airframes/twinjet1.xml"
|
|
||||||
radio="radios/cockpitSX.xml"
|
|
||||||
telemetry="telemetry/default.xml"
|
|
||||||
flight_plan="flight_plans/basic.xml"
|
|
||||||
settings="settings/light.xml settings/tuning.xml"
|
|
||||||
gui_color="#ba6293"
|
|
||||||
/>
|
|
||||||
</conf>
|
|
||||||
@@ -1,134 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
|
|
||||||
<control_panel name="paparazzi control panel">
|
|
||||||
|
|
||||||
<section name="variables">
|
|
||||||
<variable name="downlink_serial_port" value="/dev/ttyUSB0"/>
|
|
||||||
<variable name="fbw_serial_port" value="/dev/ttyS1"/>
|
|
||||||
<variable name="ap_serial_port" value="/dev/ttyS0"/>
|
|
||||||
<variable name="ivy_bus" value="127:2010"/>
|
|
||||||
<variable name="map" value="muret_UTM.xml"/>
|
|
||||||
<variable name="flight_plan" value="flight_plans/muret1.xml"/>
|
|
||||||
</section>
|
|
||||||
|
|
||||||
<section name="programs">
|
|
||||||
<program name="Server" command="sw/ground_segment/tmtc/server">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Data Link" command="sw/ground_segment/tmtc/link">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="GCS" command="sw/ground_segment/cockpit/gcs">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Flight Plan Editor" command="sw/ground_segment/cockpit/gcs -edit">
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Messages" command="sw/ground_segment/tmtc/messages">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Settings" command="sw/ground_segment/tmtc/settings">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Log Plotter" command ="sw/logalizer/plot"/>
|
|
||||||
|
|
||||||
<program name="Real-time Plotter" command ="sw/logalizer/plotter"/>
|
|
||||||
|
|
||||||
<program name="Log File Player" command="sw/logalizer/play">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Simulator" command="sw/simulator/launchsitl">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Hardware in the Loop" command="sw/simulator/simhitl">
|
|
||||||
<arg flag="-fbw" variable="fbw_serial_port"/>
|
|
||||||
<arg flag="-ap" variable="ap_serial_port"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Environment Simulator" command="sw/simulator/gaia">
|
|
||||||
<arg flag="-b" variable="ivy_bus"/>
|
|
||||||
</program>
|
|
||||||
|
|
||||||
<program name="Http Server" command="sw/ground_segment/tmtc/boa"/>
|
|
||||||
|
|
||||||
<program name="TCP controller" command="sw/ground_segment/tmtc/ivy_tcp_controller"/>
|
|
||||||
<program name="TCP aircraft" command="sw/ground_segment/tmtc/ivy_tcp_aircraft"/>
|
|
||||||
<program name="Spook" command="sw/in_progress/videolizer/spook/spook"/>
|
|
||||||
</section>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<section name="sessions">
|
|
||||||
|
|
||||||
<session name="Berlin">
|
|
||||||
<program name="Server"/>
|
|
||||||
<program name="GCS">
|
|
||||||
<arg flag="-mplayer" constant="rtsp://ricou.homelinux.org:7070/webcam"/>
|
|
||||||
<arg flag="-layout" constant="layout_berlin.xml"/>
|
|
||||||
</program>
|
|
||||||
<program name="TCP controller">
|
|
||||||
<arg flag="-h" constant="85.214.48.162"/>
|
|
||||||
</program>
|
|
||||||
</session>
|
|
||||||
|
|
||||||
<session name="Toulouse">
|
|
||||||
<program name="Data Link">
|
|
||||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
|
||||||
<arg flag="-s" constant="57600"/>
|
|
||||||
<arg flag="-aerocomm" constant=""/>
|
|
||||||
</program>
|
|
||||||
<program name="Server"/>
|
|
||||||
<program name="GCS"/>
|
|
||||||
<program name="TCP aircraft">
|
|
||||||
<arg flag="-h" constant="85.214.48.162"/>
|
|
||||||
</program>
|
|
||||||
<program name="Spook">
|
|
||||||
<arg flag="-c" constant="/home/pascal/pprz/savannah/paparazzi3/sw/in_progress/videolizer/spook/spook.conf.usbraw"/>
|
|
||||||
</program>
|
|
||||||
</session>
|
|
||||||
|
|
||||||
<session name="Hildesheim">
|
|
||||||
<program name="Data Link">
|
|
||||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
|
||||||
</program>
|
|
||||||
<program name="Server"/>
|
|
||||||
<program name="GCS">
|
|
||||||
<arg flag="-layout" constant="layout_berlin.xml"/>
|
|
||||||
<arg flag="-mplayer" constant="rtsp://localhost:7070/webcam"/>
|
|
||||||
</program>
|
|
||||||
<program name="TCP aircraft">
|
|
||||||
<arg flag="-h" constant="85.214.48.162"/>
|
|
||||||
<arg flag="-id" constant="26"/>
|
|
||||||
</program>
|
|
||||||
<program name="Spook">
|
|
||||||
<arg flag="-c" constant="/home/mum2hi/uav/paparazzi3/sw/in_progress/videolizer/spook/spook.conf"/>
|
|
||||||
</program>
|
|
||||||
</session>
|
|
||||||
|
|
||||||
|
|
||||||
<session name="simulation">
|
|
||||||
<program name="Server"/>
|
|
||||||
<program name="GCS"> </program>
|
|
||||||
<program name="Simulator">
|
|
||||||
<arg flag="-a" constant="TJ1"/>
|
|
||||||
<arg flag="-norc" constant=""/>
|
|
||||||
</program>
|
|
||||||
<program name="Simulator">
|
|
||||||
<arg flag="-a" constant="R1"/>
|
|
||||||
<arg flag="-norc" constant=""/>
|
|
||||||
</program>
|
|
||||||
</session>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
</section>
|
|
||||||
|
|
||||||
</control_panel>
|
|
||||||
@@ -1,15 +0,0 @@
|
|||||||
<!DOCTYPE layout SYSTEM "layout.dtd">
|
|
||||||
|
|
||||||
<layout width="1024" height="768">
|
|
||||||
<columns>
|
|
||||||
<rows size="400">
|
|
||||||
<widget size="300" name="strips"/>
|
|
||||||
<widget size="300" name="plugin"/>
|
|
||||||
<widget name="alarms"/>
|
|
||||||
</rows>
|
|
||||||
<rows>
|
|
||||||
<widget name="map2d"/>
|
|
||||||
<widget name="aircraft"/>
|
|
||||||
</rows>
|
|
||||||
</columns>
|
|
||||||
</layout>
|
|
||||||
@@ -1,38 +0,0 @@
|
|||||||
#
|
|
||||||
# $Id$
|
|
||||||
# Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
|
||||||
#
|
|
||||||
# This file is part of paparazzi.
|
|
||||||
#
|
|
||||||
# paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
# it under the terms of the GNU General Public License as published by
|
|
||||||
# the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
# any later version.
|
|
||||||
#
|
|
||||||
# paparazzi is distributed in the hope that it will be useful,
|
|
||||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
# GNU General Public License for more details.
|
|
||||||
#
|
|
||||||
# You should have received a copy of the GNU General Public License
|
|
||||||
# along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
# the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
# Boston, MA 02111-1307, USA.
|
|
||||||
#
|
|
||||||
|
|
||||||
# Quiet compilation
|
|
||||||
Q=@
|
|
||||||
|
|
||||||
all: fdm_step
|
|
||||||
|
|
||||||
fdm_step: fdm_step.c
|
|
||||||
gcc -g -O2 -Wall $(shell pkg-config glib-2.0 --cflags) -o $@ $^ $(shell pkg-config glib-2.0 --libs) -lglibivy
|
|
||||||
|
|
||||||
|
|
||||||
fms_steps_attitude: fms_steps_attitude.c
|
|
||||||
gcc -g -O2 -Wall $(shell pkg-config glib-2.0 --cflags) -o $@ $^ $(shell pkg-config glib-2.0 --libs) -lglibivy
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
clean:
|
|
||||||
$(Q)rm -f *~ core *.o *.bak .depend fdm_step fms_steps_attitude
|
|
||||||
@@ -1,18 +0,0 @@
|
|||||||
#ifndef BOOZ2_FMS_EXTERN_H
|
|
||||||
#define BOOZ2_FMS_EXTERN_H
|
|
||||||
|
|
||||||
#define FMS_H_MODE_RATE 0
|
|
||||||
#define FMS_H_MODE_ATTITUDE 1
|
|
||||||
#define FMS_H_MODE_SPEED 2
|
|
||||||
#define FMS_H_MODE_POS 3
|
|
||||||
|
|
||||||
#define FMS_ATTITUDE_OF_DEG(_d) ((int32_t)((_d)*0.0000546))
|
|
||||||
|
|
||||||
#define FMS_V_MODE_DIRECT 0
|
|
||||||
#define FMS_V_MODE_CLIMB 1
|
|
||||||
#define FMS_V_MODE_HEIGHT 2
|
|
||||||
|
|
||||||
//#define BOOZ2_SEND_IVY_FMS_COMMAND
|
|
||||||
|
|
||||||
|
|
||||||
#endif /* BOOZ2_FMS_EXTERN_H */
|
|
||||||
@@ -1,81 +0,0 @@
|
|||||||
/*
|
|
||||||
* fdm_step
|
|
||||||
*
|
|
||||||
*
|
|
||||||
* This program is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program; if not, write to the Free Software
|
|
||||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <glib.h>
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
|
|
||||||
#define TIMEOUT_PERIOD 100
|
|
||||||
|
|
||||||
#define DEFAULT_AC_ID 1
|
|
||||||
|
|
||||||
/* Options */
|
|
||||||
static int aircraft_id = DEFAULT_AC_ID;
|
|
||||||
static int counter;
|
|
||||||
|
|
||||||
void parse_args(int argc, char * argv[])
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
|
|
||||||
for (i = 1; i < argc; i++) {
|
|
||||||
if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
|
||||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
|
|
||||||
l_help:
|
|
||||||
printf("Usage:\n");
|
|
||||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
|
||||||
printf("Options:\n");
|
|
||||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
|
||||||
printf(" -h display this help\n");
|
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
#define STEP_VAL 127
|
|
||||||
static gboolean periodic(gpointer data __attribute__ ((unused))) {
|
|
||||||
int roll, pitch;
|
|
||||||
counter++;
|
|
||||||
pitch = 0;
|
|
||||||
roll = STEP_VAL;
|
|
||||||
if ((counter%6) >=3) roll = -roll;
|
|
||||||
|
|
||||||
IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
|
|
||||||
return TRUE;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main ( int argc, char** argv) {
|
|
||||||
|
|
||||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
|
||||||
|
|
||||||
parse_args(argc, argv);
|
|
||||||
|
|
||||||
IvyInit ("IvyFdmStep", "IvyFdmStep READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyStart("127.255.255.255");
|
|
||||||
|
|
||||||
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
|
|
||||||
|
|
||||||
g_main_loop_run(ml);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,103 +0,0 @@
|
|||||||
/*
|
|
||||||
* fms_steps_attitude
|
|
||||||
*
|
|
||||||
*
|
|
||||||
* This program is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program; if not, write to the Free Software
|
|
||||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <glib.h>
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
|
|
||||||
#include <inttypes.h>
|
|
||||||
#include "booz2_fms_extern.h"
|
|
||||||
|
|
||||||
|
|
||||||
#define TIMEOUT_PERIOD 100
|
|
||||||
#define DEFAULT_AC_ID 150
|
|
||||||
#define STEP_VAL 2
|
|
||||||
#define STEP_PERIOD 3
|
|
||||||
|
|
||||||
static int aircraft_id = DEFAULT_AC_ID;
|
|
||||||
static int counter;
|
|
||||||
|
|
||||||
static gboolean periodic(gpointer data __attribute__ ((unused)));
|
|
||||||
|
|
||||||
|
|
||||||
void parse_args(int argc, char * argv[])
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
|
|
||||||
for (i = 1; i < argc; i++) {
|
|
||||||
if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
|
||||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
|
|
||||||
l_help:
|
|
||||||
printf("Usage:\n");
|
|
||||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
|
||||||
printf("Options:\n");
|
|
||||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
|
||||||
printf(" -h display this help\n");
|
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
<field name="ac_id" type="uint8"/>
|
|
||||||
<field name="h_mode" type="uint8" values="RATE|ATTITUDE|SPEED|POS"/>
|
|
||||||
<field name="v_mode" type="uint8" values="DIRECT|CLIMB|HEIGHT"/>
|
|
||||||
<field name="pad0" type="uint8"/>
|
|
||||||
<field name="pad1" type="uint8"/>
|
|
||||||
<field name="pad2" type="uint8"/>
|
|
||||||
<field name="v_sp" type="int32"/>
|
|
||||||
<field name="h_sp" type="int32[]"/>
|
|
||||||
*/
|
|
||||||
|
|
||||||
static gboolean periodic(gpointer data __attribute__ ((unused))) {
|
|
||||||
|
|
||||||
counter++;
|
|
||||||
|
|
||||||
uint8_t h_mode = FMS_H_MODE_ATTITUDE;
|
|
||||||
uint8_t v_mode = FMS_V_MODE_DIRECT;
|
|
||||||
int32_t pitch = FMS_ATTITUDE_OF_DEG(0);
|
|
||||||
int32_t roll = FMS_ATTITUDE_OF_DEG(STEP_VAL);
|
|
||||||
int32_t yaw = FMS_ATTITUDE_OF_DEG(0);
|
|
||||||
int32_t unused = 0;
|
|
||||||
if ((counter%(2*STEP_PERIOD)) >= STEP_PERIOD) roll = -roll;
|
|
||||||
|
|
||||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d d %d %d %d,%d,%d", aircraft_id, h_mode, v_mode, unused, unused, unused, unused, roll, pitch, yaw);
|
|
||||||
return TRUE;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main ( int argc, char** argv) {
|
|
||||||
|
|
||||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
|
||||||
|
|
||||||
parse_args(argc, argv);
|
|
||||||
|
|
||||||
IvyInit ("IvyBooz2FmsAttStep", "IvyBooz2FmsAttStep READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyStart("127.255.255.255");
|
|
||||||
|
|
||||||
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
|
|
||||||
|
|
||||||
g_main_loop_run(ml);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,89 +0,0 @@
|
|||||||
/*
|
|
||||||
* fdm_step_position
|
|
||||||
*
|
|
||||||
*
|
|
||||||
* This program is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program; if not, write to the Free Software
|
|
||||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <glib.h>
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
|
|
||||||
#define TIMEOUT_PERIOD 100
|
|
||||||
#define DEFAULT_AC_ID 150
|
|
||||||
|
|
||||||
static int aircraft_id = DEFAULT_AC_ID;
|
|
||||||
static int counter;
|
|
||||||
|
|
||||||
static gboolean periodic(gpointer data __attribute__ ((unused)));
|
|
||||||
static void on_FMS_info(IvyClientPtr app, void *user_data, int argc, char *argv[]);
|
|
||||||
|
|
||||||
void parse_args(int argc, char * argv[])
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
|
|
||||||
for (i = 1; i < argc; i++) {
|
|
||||||
if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
|
||||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
|
|
||||||
l_help:
|
|
||||||
printf("Usage:\n");
|
|
||||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
|
||||||
printf("Options:\n");
|
|
||||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
|
||||||
printf(" -h display this help\n");
|
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void on_FMS_state(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
// guint ac_id = atoi(argv[0]);
|
|
||||||
// float estimator_phi = atof(argv[1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#define STEP_VAL 32
|
|
||||||
static gboolean periodic(gpointer data __attribute__ ((unused))) {
|
|
||||||
int roll, pitch;
|
|
||||||
counter++;
|
|
||||||
pitch = 0;
|
|
||||||
roll = STEP_VAL;
|
|
||||||
if ((counter%6) >=3) roll = -roll;
|
|
||||||
|
|
||||||
IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
|
|
||||||
return TRUE;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main ( int argc, char** argv) {
|
|
||||||
|
|
||||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
|
||||||
|
|
||||||
parse_args(argc, argv);
|
|
||||||
|
|
||||||
IvyInit ("IvyFdmStep", "IvyFdmStep READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyStart("127.255.255.255");
|
|
||||||
|
|
||||||
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
|
|
||||||
|
|
||||||
g_main_loop_run(ml);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,14 +0,0 @@
|
|||||||
|
|
||||||
# Quiet compilation
|
|
||||||
Q=@
|
|
||||||
|
|
||||||
|
|
||||||
CC = gcc
|
|
||||||
CFLAGS=-g -O2 -Wall $(shell pkg-config gtk+-2.0 --cflags)
|
|
||||||
LDFLAGS=$(shell pkg-config gtk+-2.0 --libs) -lglibivy -lm -lgtkdatabox
|
|
||||||
|
|
||||||
ir_calib : main.c calibrator.c gui.c
|
|
||||||
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
|
|
||||||
|
|
||||||
clean:
|
|
||||||
$(Q)rm -f *~ ir_calib
|
|
||||||
@@ -1,163 +0,0 @@
|
|||||||
#include "calibrator.h"
|
|
||||||
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
|
|
||||||
#define DEG_OF_RAD(a) ((a)/M_PI*180.)
|
|
||||||
#define RAD_OF_DEG(a) ((a)*M_PI/180.)
|
|
||||||
#define NORM_ANGLE_RAD(a) { while ((a) < -M_PI ) (a) += 2 * M_PI; while ((a) > M_PI ) (a) -= 2 * M_PI; }
|
|
||||||
|
|
||||||
#define AC_ID 43
|
|
||||||
#define IR_ROLL_NEUTRAL 0.
|
|
||||||
#define IR_360_LATERAL_CORRECTION 1.
|
|
||||||
#define IR_360_VERTICAL_CORRECTION 1.
|
|
||||||
#define IR_CORRECTION_RIGHT 1.
|
|
||||||
#define IR_CORRECTION_LEFT 1.
|
|
||||||
|
|
||||||
#define DT 0.25
|
|
||||||
|
|
||||||
#define g 9.81
|
|
||||||
|
|
||||||
float est_airspeed;
|
|
||||||
float est_wind_dir;
|
|
||||||
float est_wind_speed;
|
|
||||||
float est_wind_east;
|
|
||||||
float est_wind_north;
|
|
||||||
|
|
||||||
float gnd_ir_phi;
|
|
||||||
float estimator_phi;
|
|
||||||
|
|
||||||
|
|
||||||
float gps_gs_east;
|
|
||||||
float gps_gs_north;
|
|
||||||
float gps_gs_angle;
|
|
||||||
float gps_gs_norm;
|
|
||||||
|
|
||||||
float gps_as_east;
|
|
||||||
float gps_as_north;
|
|
||||||
float gps_as_angle;
|
|
||||||
float gps_as_norm;
|
|
||||||
|
|
||||||
float gps_phi;
|
|
||||||
|
|
||||||
#define NB_POINTS 121
|
|
||||||
float estimator_phi_by_degres[NB_POINTS];
|
|
||||||
float alpha = 0.5;
|
|
||||||
|
|
||||||
static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]);
|
|
||||||
static void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]);
|
|
||||||
static void on_Wind(IvyClientPtr app, void *user_data, int argc, char *argv[]);
|
|
||||||
static void on_IrSensors(IvyClientPtr app, void *user_data, int argc, char *argv[]);
|
|
||||||
static void reset_average(void);
|
|
||||||
|
|
||||||
|
|
||||||
static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
//guint ac_id = atoi(argv[0]);
|
|
||||||
estimator_phi = RAD_OF_DEG(atof(argv[1]));
|
|
||||||
//g_message("attitude %d %f", ac_id, estimator_phi);
|
|
||||||
int gps_phi_deg = round(DEG_OF_RAD(gps_phi));
|
|
||||||
if (fabs(gps_phi_deg) <= NB_POINTS/2) {
|
|
||||||
unsigned int idx = gps_phi_deg + NB_POINTS/2;
|
|
||||||
estimator_phi_by_degres[idx] = (1-alpha) * estimator_phi_by_degres[idx] + alpha * DEG_OF_RAD(estimator_phi);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
unsigned int ac_id = atoi(argv[0]);
|
|
||||||
float course = atof(argv[4]);
|
|
||||||
float speed = atof(argv[6]);
|
|
||||||
|
|
||||||
gps_gs_norm = speed / 100.;
|
|
||||||
gps_gs_angle = RAD_OF_DEG(90. - course / 10.);
|
|
||||||
NORM_ANGLE_RAD( gps_gs_angle );
|
|
||||||
|
|
||||||
gps_gs_east = gps_gs_norm * cos(gps_gs_angle);
|
|
||||||
gps_gs_north = gps_gs_norm * sin(gps_gs_angle);
|
|
||||||
|
|
||||||
gps_as_east = gps_gs_east - est_wind_east;
|
|
||||||
gps_as_north = gps_gs_north - est_wind_north;
|
|
||||||
|
|
||||||
gps_as_angle = atan2(gps_as_north,gps_as_east);
|
|
||||||
gps_as_norm = sqrt(gps_as_east*gps_as_east + gps_as_north*gps_as_north);
|
|
||||||
|
|
||||||
static float old_psi = 0.;
|
|
||||||
float delta_psi = gps_as_angle - old_psi;
|
|
||||||
old_psi = gps_as_angle;
|
|
||||||
NORM_ANGLE_RAD(delta_psi);
|
|
||||||
|
|
||||||
/* tan(phi) = v^2 / (R*g) */
|
|
||||||
/* R = (V * dt) / dpsi */
|
|
||||||
|
|
||||||
if (fabs(delta_psi) < 1e-6)
|
|
||||||
delta_psi = copysign(1e-6, delta_psi);
|
|
||||||
|
|
||||||
float R = -gps_as_norm * DT / delta_psi;
|
|
||||||
|
|
||||||
gps_phi = atan(gps_as_norm * gps_as_norm / R / g);
|
|
||||||
|
|
||||||
printf("gps %d % 3.1f \t% 3.0f \t%.1f \t%.1f \t%.1f\n", ac_id, DEG_OF_RAD(delta_psi), R, DEG_OF_RAD(gps_phi), DEG_OF_RAD(estimator_phi), DEG_OF_RAD(gnd_ir_phi));
|
|
||||||
}
|
|
||||||
|
|
||||||
void on_Wind(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
//guint ac_id = atoi(argv[1]);
|
|
||||||
est_wind_dir = atof(argv[2]);
|
|
||||||
est_wind_speed = atof(argv[3]);
|
|
||||||
est_airspeed = atof(argv[4]);
|
|
||||||
|
|
||||||
float w_dir_rad =RAD_OF_DEG(270. - est_wind_dir);
|
|
||||||
NORM_ANGLE_RAD( w_dir_rad );
|
|
||||||
|
|
||||||
est_wind_east = est_wind_speed * cos( w_dir_rad );
|
|
||||||
est_wind_north = est_wind_speed * sin( w_dir_rad );
|
|
||||||
|
|
||||||
//g_message("wind %d %f %f %f", ac_id, w_dir, w_speed, ac_aspeed);
|
|
||||||
//g_message("wind %f %f %f", w_dir_rad, est_wind_east, est_wind_north);
|
|
||||||
}
|
|
||||||
|
|
||||||
void on_IrSensors(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
//guint ac_id = atoi(argv[0]);
|
|
||||||
float lateral = atof(argv[2]);
|
|
||||||
float vertical = atof(argv[3]);
|
|
||||||
|
|
||||||
float ir_roll = lateral * IR_360_LATERAL_CORRECTION;
|
|
||||||
float ir_top = vertical * IR_360_LATERAL_CORRECTION;
|
|
||||||
|
|
||||||
gnd_ir_phi = atan2(ir_roll, ir_top) - IR_ROLL_NEUTRAL;
|
|
||||||
|
|
||||||
if (gnd_ir_phi >= 0)
|
|
||||||
gnd_ir_phi *= IR_CORRECTION_RIGHT;
|
|
||||||
else
|
|
||||||
gnd_ir_phi *= IR_CORRECTION_LEFT;
|
|
||||||
|
|
||||||
// g_message("ir_sensors %d %.0f %.0f (%.1f)", ac_id, lateral, vertical, DEG_OF_RAD(phi));
|
|
||||||
}
|
|
||||||
|
|
||||||
static void reset_average(void) {
|
|
||||||
|
|
||||||
int i;
|
|
||||||
for (i=0; i<NB_POINTS; i++) {
|
|
||||||
estimator_phi_by_degres[i] = i-NB_POINTS/2;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void calibrator_init(void) {
|
|
||||||
|
|
||||||
IvyInit ("IrCalib", "IrCalib READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyBindMsg(on_Attitude, NULL, "^(\\S*) ATTITUDE (\\S*) (\\S*) (\\S*)");
|
|
||||||
IvyBindMsg(on_GPS, NULL, "^(\\S*) GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
|
||||||
IvyBindMsg(on_Wind, NULL, "^(\\S*) WIND (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
|
||||||
IvyBindMsg(on_IrSensors, NULL, "^(\\S*) IR_SENSORS (\\S*) (\\S*) (\\S*)");
|
|
||||||
IvyStart("127.255.255.255");
|
|
||||||
|
|
||||||
reset_average();
|
|
||||||
}
|
|
||||||
|
|
||||||
float* calibrator_get_values(void) {
|
|
||||||
return estimator_phi_by_degres;
|
|
||||||
}
|
|
||||||
@@ -1,8 +0,0 @@
|
|||||||
#ifndef CALIBRATOR_H
|
|
||||||
#define CALIBRATOR_H
|
|
||||||
|
|
||||||
|
|
||||||
extern void calibrator_init(void);
|
|
||||||
extern float* calibrator_get_values(void);
|
|
||||||
|
|
||||||
#endif /* CALIBRATOR_H */
|
|
||||||
@@ -1,68 +0,0 @@
|
|||||||
#include "gui.h"
|
|
||||||
|
|
||||||
#include <gtkdatabox.h>
|
|
||||||
#include <gtkdatabox_points.h>
|
|
||||||
#include <gtkdatabox_lines.h>
|
|
||||||
#include <gtkdatabox_bars.h>
|
|
||||||
#include <gtkdatabox_grid.h>
|
|
||||||
#include <gtkdatabox_cross_simple.h>
|
|
||||||
#include <gtkdatabox_markers.h>
|
|
||||||
|
|
||||||
#define NB_POINTS 121
|
|
||||||
|
|
||||||
gfloat *X;
|
|
||||||
gfloat *Y1;
|
|
||||||
gfloat *Y2;
|
|
||||||
|
|
||||||
static GtkWidget* databox;
|
|
||||||
|
|
||||||
GtkWidget* gui_init(void) {
|
|
||||||
|
|
||||||
GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
|
|
||||||
gtk_widget_set_size_request (window, 640, 400);
|
|
||||||
|
|
||||||
GtkWidget *vbox1 = gtk_vbox_new (FALSE, 0);
|
|
||||||
gtk_container_add (GTK_CONTAINER (window), vbox1);
|
|
||||||
|
|
||||||
GtkWidget *frame = gtk_frame_new ("Foo");
|
|
||||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
|
||||||
|
|
||||||
databox = gtk_databox_new ();
|
|
||||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
|
||||||
|
|
||||||
X = g_new0 (gfloat, NB_POINTS);
|
|
||||||
Y1 = g_new0 (gfloat, NB_POINTS);
|
|
||||||
Y2 = g_new0 (gfloat, NB_POINTS);
|
|
||||||
guint i;
|
|
||||||
for (i=0; i<NB_POINTS; i++){
|
|
||||||
X[i] = (gfloat)i - NB_POINTS/2;
|
|
||||||
Y1[i] = (gfloat)i -NB_POINTS/2;
|
|
||||||
Y2[i] = (gfloat)i -NB_POINTS/2;
|
|
||||||
}
|
|
||||||
|
|
||||||
GdkColor cblack = {0, 0, 0, 0};
|
|
||||||
GdkColor cred = {0, 65535, 0, 0};
|
|
||||||
|
|
||||||
GtkDataboxGraph *graph1 = gtk_databox_lines_new (NB_POINTS, X, Y1, &cblack, 2);
|
|
||||||
gtk_databox_graph_add (GTK_DATABOX (databox), graph1);
|
|
||||||
|
|
||||||
GtkDataboxGraph *graph2 = gtk_databox_lines_new (NB_POINTS, X, Y2, &cred, 2);
|
|
||||||
gtk_databox_graph_add (GTK_DATABOX (databox), graph2);
|
|
||||||
|
|
||||||
GtkDataboxGraph *grid = gtk_databox_grid_new (11, 11, &cblack, 1);
|
|
||||||
gtk_databox_graph_add (GTK_DATABOX (databox), grid);
|
|
||||||
|
|
||||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
|
||||||
|
|
||||||
gtk_widget_show_all(window);
|
|
||||||
return window;
|
|
||||||
}
|
|
||||||
|
|
||||||
void gui_update(gfloat* values) {
|
|
||||||
guint i;
|
|
||||||
for (i=0; i<NB_POINTS; i++){
|
|
||||||
Y2[i] = values[i];
|
|
||||||
}
|
|
||||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
|
||||||
}
|
|
||||||
@@ -1,10 +0,0 @@
|
|||||||
#ifndef GUI_H
|
|
||||||
#define GUI_H
|
|
||||||
|
|
||||||
#include <gtk/gtk.h>
|
|
||||||
|
|
||||||
extern GtkWidget* gui_init(void);
|
|
||||||
|
|
||||||
extern void gui_update(gfloat* values);
|
|
||||||
|
|
||||||
#endif /* GUI_H */
|
|
||||||
@@ -1,35 +0,0 @@
|
|||||||
#include <glib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
|
|
||||||
#include "calibrator.h"
|
|
||||||
#include "gui.h"
|
|
||||||
|
|
||||||
gboolean timeout_callback(gpointer data) {
|
|
||||||
|
|
||||||
float* values = calibrator_get_values();
|
|
||||||
gui_update(values);
|
|
||||||
return TRUE;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main ( int argc, char** argv) {
|
|
||||||
|
|
||||||
// GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
|
||||||
|
|
||||||
gtk_init(&argc, &argv);
|
|
||||||
|
|
||||||
g_timeout_add(500, timeout_callback, NULL);
|
|
||||||
|
|
||||||
calibrator_init();
|
|
||||||
|
|
||||||
gui_init();
|
|
||||||
|
|
||||||
//g_main_loop_run(ml);
|
|
||||||
gtk_main();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,19 +0,0 @@
|
|||||||
#
|
|
||||||
# Makefile
|
|
||||||
#
|
|
||||||
|
|
||||||
# Quiet compilation
|
|
||||||
Q=@
|
|
||||||
|
|
||||||
APP = i2c_usb
|
|
||||||
|
|
||||||
all: $(APP)
|
|
||||||
|
|
||||||
clean:
|
|
||||||
$(Q)rm -f $(APP)
|
|
||||||
|
|
||||||
$(APP): $(APP).c
|
|
||||||
$(CC) -Wall -o $@ $(APP).c -lusb
|
|
||||||
|
|
||||||
install:
|
|
||||||
install $(APP) $(DESTDIR)/usr/bin
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -1,52 +0,0 @@
|
|||||||
i2c-tiny-usb test application - http://www.harbaum.org/till/i2c_tiny_usb
|
|
||||||
------------------------------------------------------------------------
|
|
||||||
|
|
||||||
Adapted to Melexis 90614 IR sensors.
|
|
||||||
|
|
||||||
This simple test application is meant to demonstrate libusb
|
|
||||||
interfacing to the i2c-tiny-usb interface.
|
|
||||||
|
|
||||||
This is no useful application, if you are only interesting in
|
|
||||||
using the i2c-tiny-usb interface in your linux box please
|
|
||||||
use the kernel driver.
|
|
||||||
|
|
||||||
Linux
|
|
||||||
-----
|
|
||||||
|
|
||||||
This demo application has been developed under and for linux. Just
|
|
||||||
make sure you have libusb installed. To use this program just
|
|
||||||
compile by typing "make" and run the resulting i2c_usb.
|
|
||||||
|
|
||||||
Be sure that the i2c-tiny-usb kernel driver is not loaded while
|
|
||||||
running the test application. Otherwise the test application will
|
|
||||||
fail with the follwing error message:
|
|
||||||
|
|
||||||
USB error: could not claim interface 0: Device or resource busy
|
|
||||||
|
|
||||||
This is due to the fact that no two drivers may access the interface
|
|
||||||
at the same time.
|
|
||||||
|
|
||||||
Windows
|
|
||||||
-------
|
|
||||||
|
|
||||||
This program can be compiled for windows. This has been tested
|
|
||||||
under Linux using xmingw and the windows port of libusb
|
|
||||||
(see http://libusb-win32.sourceforge.net). To install the
|
|
||||||
driver plug the device in and install the driver from
|
|
||||||
the win directory. Then run testapp/i2c_usb.exe
|
|
||||||
|
|
||||||
This program may also be compiled under windows using cygwin or
|
|
||||||
mingw (which is part of cygwin). In order to use cygwin simply
|
|
||||||
copy usb.h win32-linusb to /cygwin/usr/include and libusb.a to
|
|
||||||
/cygwin/lib and do a "make -f Makefile.cygwin". Don't forget to
|
|
||||||
distribute /cygwin/bin/cygwin1.dll with your file to allow it to
|
|
||||||
run in non-cygwin environments as well. No dll is required when using
|
|
||||||
mingw. In that case copy usb.h to /cygwin/usr/include/mingw and
|
|
||||||
libusb.a to /cygwin/lib/mingw. Finally do a "make -f Makefile.mingw".
|
|
||||||
|
|
||||||
MacOS X
|
|
||||||
-------
|
|
||||||
|
|
||||||
The program can be compiled under MacOS as well. The fink version
|
|
||||||
of linusb has to be installed and a simple "make -f Makefile.macos"
|
|
||||||
will build the native MacOS X version.
|
|
||||||
@@ -1,80 +0,0 @@
|
|||||||
/*
|
|
||||||
http://modestmaps.mapstraction.com/trac/wiki/TileNamingConventions
|
|
||||||
http://www.maptiler.org/google-maps-coordinates-tile-bounds-projection/
|
|
||||||
http://wiki.openstreetmap.org/wiki/Slippy_map_tilenames3
|
|
||||||
|
|
||||||
gcc osm.c -lm -Wall -o osm
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
int long2tile(double lon, int z)
|
|
||||||
{
|
|
||||||
return (int)(floor((lon + 180.0) / 360.0 * pow(2.0, z)));
|
|
||||||
}
|
|
||||||
|
|
||||||
int lat2tile(double lat, int z)
|
|
||||||
{
|
|
||||||
return (int)(floor((1.0 - log( tan(lat * M_PI/180.0) + 1.0 / cos(lat * M_PI/180.0)) / M_PI) / 2.0 * pow(2.0, z)));
|
|
||||||
}
|
|
||||||
|
|
||||||
void gm_quadtree(int x, int y, int z, char *buffer)
|
|
||||||
{
|
|
||||||
static const char *const quadrant = "qrts";
|
|
||||||
int i;
|
|
||||||
char *ptr = buffer;
|
|
||||||
|
|
||||||
*ptr++ = 't';
|
|
||||||
for (i = z-1; i >= 0; i--)
|
|
||||||
{
|
|
||||||
int xbit = (x >> i) & 1;
|
|
||||||
int ybit = (y >> i) & 1;
|
|
||||||
*ptr++ = quadrant[xbit + 2 * ybit];
|
|
||||||
}
|
|
||||||
*ptr++ = '\0';
|
|
||||||
}
|
|
||||||
|
|
||||||
void ms_quadtree(int x, int y, int z, char *buffer)
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
char *ptr = buffer;
|
|
||||||
|
|
||||||
for (i = z; i > 0; i--)
|
|
||||||
{
|
|
||||||
int mask = 1 << (i - 1);
|
|
||||||
char digit = '0';
|
|
||||||
if ((x & mask) != 0)
|
|
||||||
{
|
|
||||||
digit+=1;
|
|
||||||
}
|
|
||||||
if ((y & mask) != 0)
|
|
||||||
{
|
|
||||||
digit+=2;
|
|
||||||
}
|
|
||||||
*ptr++ = digit;
|
|
||||||
}
|
|
||||||
*ptr++ = '\0';
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(void)
|
|
||||||
{
|
|
||||||
int x, y, z = 14;
|
|
||||||
double lat = 52.26483, lon = 9.99394;
|
|
||||||
char ms_qkey[32];
|
|
||||||
char gm_qkey[32];
|
|
||||||
|
|
||||||
x = long2tile(lon, z);
|
|
||||||
y = lat2tile(lat, z);
|
|
||||||
ms_quadtree(x, y, z, ms_qkey);
|
|
||||||
gm_quadtree(x, y, z, gm_qkey);
|
|
||||||
|
|
||||||
printf("http://tile.openstreetmap.org/%d/%d/%d.png\n", z, x, y);
|
|
||||||
printf("http://khm0.google.com/kh/v=45&x=%d&s=&y=%d&z=%d\n", x, y, z);
|
|
||||||
// printf("http://kh.google.com/kh?v=3&t=%s\n", gm_qkey);
|
|
||||||
// printf("http://mt1.google.com/mt/v=ap.95&hl=en&x=%d&y=%d&z=%d&s=G\n", x, y, z);
|
|
||||||
printf("http://a0.ortho.tiles.virtualearth.net/tiles/a%s.jpeg?g=%d\n", ms_qkey, z+32);
|
|
||||||
printf("http://r0.ortho.tiles.virtualearth.net/tiles/r%s.png?g=%d\n", ms_qkey, z+32);
|
|
||||||
|
|
||||||
return(0);
|
|
||||||
};
|
|
||||||
@@ -1,20 +0,0 @@
|
|||||||
# Quiet compilation
|
|
||||||
Q=@
|
|
||||||
|
|
||||||
CC = gcc
|
|
||||||
CFLAGS=-g -O2 -Wall $(shell pkg-config gtk+-2.0 --cflags) -I../../../var/MB
|
|
||||||
LDFLAGS=$(shell pkg-config gtk+-2.0 --libs) -s -lglibivy
|
|
||||||
|
|
||||||
motor_bench : main.c
|
|
||||||
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
|
|
||||||
|
|
||||||
# -lgtkdatabox
|
|
||||||
|
|
||||||
LOG=mb_log.txt
|
|
||||||
|
|
||||||
plot_test:
|
|
||||||
scilab -f test.sce -args $(LOG)
|
|
||||||
|
|
||||||
clean:
|
|
||||||
$(Q)rm -f *~ motor_bench
|
|
||||||
|
|
||||||
Binary file not shown.
@@ -1,56 +0,0 @@
|
|||||||
clear();
|
|
||||||
getf('mb_utils.sci');
|
|
||||||
|
|
||||||
filename = "data/steps_stout_aero_geared.txt";
|
|
||||||
|
|
||||||
[time, throttle, rpm, amp, thrust, torque] = read_mb_log(filename);
|
|
||||||
|
|
||||||
|
|
||||||
// design an iir low pass with cutoff freq at 125Hz
|
|
||||||
hz=iir(3,'lp','ellip',[0.5 0.5],[.01 .01]);
|
|
||||||
|
|
||||||
//[hzm,fr]=frmag(hz,256);
|
|
||||||
//plot2d(fr',hzm',3)
|
|
||||||
|
|
||||||
[rpm_lp] = rtitr(hz(2), hz(3), rpm);
|
|
||||||
|
|
||||||
|
|
||||||
//if 0
|
|
||||||
xbasc();
|
|
||||||
subplot(2,1,1)
|
|
||||||
xtitle('Throttle');
|
|
||||||
plot(time(30:length(time)), throttle(30:length(throttle)), 'b-');
|
|
||||||
subplot(2,1,2)
|
|
||||||
plot(time(30:length(time)), rpm(30:length(rpm)), 'r-');
|
|
||||||
plot(time(30:length(time)), rpm_lp(30:length(rpm_lp)), 'g-');
|
|
||||||
//end
|
|
||||||
|
|
||||||
s_avg = 30;
|
|
||||||
e_avg = 118;
|
|
||||||
rpm0 = mean(rpm(s_avg:e_avg));
|
|
||||||
|
|
||||||
plot(time(s_avg:e_avg), rpm0*ones(1,e_avg - s_avg + 1), 'b-');
|
|
||||||
|
|
||||||
my_kv = 100000;
|
|
||||||
my_taukq = - 0.0003313; // plain wrong it seems :(
|
|
||||||
my_tau = 0.3; //
|
|
||||||
my_kq = my_taukq / my_tau; //
|
|
||||||
|
|
||||||
|
|
||||||
time_ode = time(s_avg:length(time));
|
|
||||||
u_ode = throttle(s_avg:length(throttle));
|
|
||||||
|
|
||||||
|
|
||||||
function rpm_dot = my_mot_ode(t, rpm)
|
|
||||||
if ((t >= 389.7960)& (t < 390.7960))
|
|
||||||
thr = 0.7;
|
|
||||||
else
|
|
||||||
thr = 0.6;
|
|
||||||
end
|
|
||||||
rpm_dot = mot_ode(my_tau, my_kq, my_kv, thr);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
rpm_sim = ode([rpm0], time(1), time_ode, my_mot_ode);
|
|
||||||
|
|
||||||
plot(time_ode, rpm_sim, 'r-');
|
|
||||||
@@ -1,108 +0,0 @@
|
|||||||
clear();
|
|
||||||
getf('mb_utils.sci');
|
|
||||||
|
|
||||||
rank=[];
|
|
||||||
time_s=[];
|
|
||||||
time_r=[];
|
|
||||||
throttle_raw=[];
|
|
||||||
tacho_raw=[];
|
|
||||||
|
|
||||||
u=mopen('data/xxx_prbs.txt','r');
|
|
||||||
while meof(u) == 0,
|
|
||||||
line = mgetl(u, 1);
|
|
||||||
if strindex(line, '#') ~= 1 & length(line) ~= 0,
|
|
||||||
[nb_scan, _rank, _time_s, _time_r, _throttle_raw, _tacho_raw] = msscanf(1, line, '%x %x %x %x %x');
|
|
||||||
if nb_scan == 5,
|
|
||||||
rank = [rank _rank];
|
|
||||||
time_s = [time_s _time_s];
|
|
||||||
time_r = [time_r _time_r];
|
|
||||||
throttle_raw = [throttle_raw _throttle_raw];
|
|
||||||
tacho_raw = [tacho_raw _tacho_raw];
|
|
||||||
end
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
mclose(u);
|
|
||||||
|
|
||||||
|
|
||||||
// extract experiment
|
|
||||||
//EXPE_LEN = 4096;
|
|
||||||
EXPE_LEN = 100;
|
|
||||||
EXPE_START = 41;
|
|
||||||
rank = rank(EXPE_START:EXPE_START+EXPE_LEN);
|
|
||||||
time_s = time_s(EXPE_START:EXPE_START+EXPE_LEN);
|
|
||||||
time_r = time_r(EXPE_START:EXPE_START+EXPE_LEN);
|
|
||||||
throttle_raw = throttle_raw(EXPE_START:EXPE_START+EXPE_LEN);
|
|
||||||
tacho_raw = tacho_raw(EXPE_START:EXPE_START+EXPE_LEN);
|
|
||||||
|
|
||||||
// compute time
|
|
||||||
TICK_PER_S = 15625;
|
|
||||||
time = time_s + time_r / TICK_PER_S;
|
|
||||||
DT = (time(length(time)) - time(1)) / (length(time) - 1)
|
|
||||||
F_EC = 1 / DT
|
|
||||||
|
|
||||||
// normalise throttle
|
|
||||||
MAX_PPRZ = 9600;
|
|
||||||
throttle = throttle_raw / MAX_PPRZ;
|
|
||||||
|
|
||||||
// compute rpms
|
|
||||||
TICK_PER_ROTATION = 36;
|
|
||||||
rpm = tacho_raw / DT / TICK_PER_ROTATION * 60;
|
|
||||||
|
|
||||||
// integrate ODE
|
|
||||||
//kv0 = 100000;
|
|
||||||
kv0 = 22000;
|
|
||||||
//taukq = 0.0003313; //
|
|
||||||
taukq = 0.00001; //
|
|
||||||
//tau0 = 0.3; //
|
|
||||||
tau0 = 0.1; //
|
|
||||||
kq0 = taukq / tau0; //
|
|
||||||
|
|
||||||
rpm0 = rpm(1);
|
|
||||||
|
|
||||||
//idx = 1;
|
|
||||||
function rpm_dot = prbs_mot_ode(t, rpm)
|
|
||||||
idx=1;
|
|
||||||
while (time(idx) < t & idx < length(throttle)), idx=idx+1, end
|
|
||||||
// printf('%f %d %f\n', t, idx, time(idx));
|
|
||||||
rpm_dot = mot_ode(my_tau, my_kq, my_kv, throttle(idx), rpm);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
p0 = [tau0; kv0; kq0];
|
|
||||||
|
|
||||||
|
|
||||||
function e = err_prbs(p,z)
|
|
||||||
my_tau = p(1);
|
|
||||||
my_kv = p(2);
|
|
||||||
my_kq = p(3);
|
|
||||||
rpm_sim = ode([rpm0], time(1), time, prbs_mot_ode);
|
|
||||||
_diff = rpm_sim - rpm;
|
|
||||||
sq_diff = _diff^2;
|
|
||||||
e = sum(sq_diff);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
if 0
|
|
||||||
my_tau = tau0;
|
|
||||||
my_kv = kv0;
|
|
||||||
my_kq = kq0;
|
|
||||||
rpm_sim = ode([rpm0], time(1), time, prbs_mot_ode);
|
|
||||||
else
|
|
||||||
Z = [ 1; 1]; //unused...
|
|
||||||
[p, err] = datafit(err_prbs, Z, p0)
|
|
||||||
my_tau = p(1);
|
|
||||||
my_kv = p(2);
|
|
||||||
my_kq = p(3);
|
|
||||||
rpm_sim = ode([rpm0], time(1), time, prbs_mot_ode);
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
xbasc();
|
|
||||||
subplot(2,1,1);
|
|
||||||
xtitle('Throttle');
|
|
||||||
plot(time, throttle, 'b-');
|
|
||||||
|
|
||||||
subplot(2,1,2);
|
|
||||||
plot(time, rpm, 'r-');
|
|
||||||
plot(time, rpm_sim, 'b-');
|
|
||||||
@@ -1,35 +0,0 @@
|
|||||||
clear();
|
|
||||||
getf('mb_utils.sci');
|
|
||||||
|
|
||||||
load('averaged_ramp_geared.dat','av_throttle','av_rpm')
|
|
||||||
|
|
||||||
// extract only data around operating point
|
|
||||||
//i_s = 20;
|
|
||||||
//i_e = 90;
|
|
||||||
i_s = 5;
|
|
||||||
i_e = 90;
|
|
||||||
t_part = av_throttle(i_s:i_e);
|
|
||||||
r_part = av_rpm(i_s:i_e);
|
|
||||||
|
|
||||||
// tau_kq kv dv
|
|
||||||
p0=[0.00001; 20000];
|
|
||||||
Z = [r_part; t_part];
|
|
||||||
//[p, err] = datafit(err_mot_lin, Z, p0);
|
|
||||||
|
|
||||||
[p, err] = datafit(err_mot_ric, Z, p0)
|
|
||||||
//p = p0;
|
|
||||||
|
|
||||||
xbasc();
|
|
||||||
//subplot(3,1,1);
|
|
||||||
xtitle('Rpms vs Throttle');
|
|
||||||
plot2d(av_throttle,av_rpm);
|
|
||||||
plot2d(t_part,r_part, 4);
|
|
||||||
|
|
||||||
|
|
||||||
r_fit = [];
|
|
||||||
for i=1:length(av_throttle)
|
|
||||||
// r_fit = [r_fit mot_lin(av_throttle(i), p)];
|
|
||||||
r_fit = [r_fit mot_ric_stat(av_throttle(i), p)];
|
|
||||||
end
|
|
||||||
|
|
||||||
plot2d(av_throttle,r_fit, 3);
|
|
||||||
@@ -1,445 +0,0 @@
|
|||||||
#include <gtk/gtk.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
|
|
||||||
#define ASTECH 1
|
|
||||||
|
|
||||||
#include "tuning.h"
|
|
||||||
|
|
||||||
//#include "sliding_plot.h"
|
|
||||||
|
|
||||||
#define MB_MODES_IDLE 0
|
|
||||||
#define MB_MODES_MANUAL 1
|
|
||||||
#define MB_MODES_RAMP 2
|
|
||||||
#define MB_MODES_STEP 3
|
|
||||||
#define MB_MODES_PRBS 4
|
|
||||||
#define MB_MODES_SINE 5
|
|
||||||
#define MB_MODES_FIXED_RPM 6
|
|
||||||
|
|
||||||
#define AS_MOT_FRONT 0
|
|
||||||
#define AS_MOT_BACK 1
|
|
||||||
#define AS_MOT_LEFT 2
|
|
||||||
#define AS_MOT_RIGHT 3
|
|
||||||
|
|
||||||
#define AS_CMD_TEST_ADDR 1
|
|
||||||
#define AS_CMD_REVERSE 2
|
|
||||||
#define AS_CMD_SET_ADDR 3
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
const guint mb_id = 158;
|
|
||||||
|
|
||||||
struct motor_bench_state {
|
|
||||||
guint mode;
|
|
||||||
double time;
|
|
||||||
double throttle;
|
|
||||||
double rpms;
|
|
||||||
double amps;
|
|
||||||
double thrust;
|
|
||||||
double torque;
|
|
||||||
double av_rpm;
|
|
||||||
double av_throttle;
|
|
||||||
double av_thrust;
|
|
||||||
double av_amps;
|
|
||||||
GIOChannel* log_channel;
|
|
||||||
GIOChannel* log_channel_static;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct motor_bench_gui {
|
|
||||||
GtkWidget* lab_time;
|
|
||||||
GtkWidget* lab_throttle;
|
|
||||||
GtkWidget* lab_rpms;
|
|
||||||
GtkWidget* lab_amps;
|
|
||||||
GtkWidget* lab_thrust;
|
|
||||||
GtkWidget* lab_torque;
|
|
||||||
GtkWidget* entry_log;
|
|
||||||
};
|
|
||||||
|
|
||||||
static void on_mode_changed (GtkRadioButton *radiobutton, gpointer user_data);
|
|
||||||
static void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]);
|
|
||||||
static gboolean timeout_callback(gpointer data);
|
|
||||||
static void on_log_button_toggled (GtkWidget *widget, gpointer data);
|
|
||||||
|
|
||||||
static void on_as_test_button_clicked (GtkWidget *widget, gpointer data);
|
|
||||||
static void on_as_reverse_button_clicked (GtkWidget *widget, gpointer data);
|
|
||||||
static void on_as_addr_changed (GtkRadioButton *radiobutton, gpointer user_data);
|
|
||||||
|
|
||||||
|
|
||||||
static GtkWidget* build_gui ( void );
|
|
||||||
|
|
||||||
static struct motor_bench_state mb_state;
|
|
||||||
static struct motor_bench_gui mb_gui;
|
|
||||||
|
|
||||||
static void on_mode_changed (GtkRadioButton *radiobutton, gpointer user_data) {
|
|
||||||
if (!gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)))
|
|
||||||
return;
|
|
||||||
guint mode = (guint)user_data;
|
|
||||||
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_MODES_MODE, mode);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if 0
|
|
||||||
void on_scale_value_changed (GtkScale *scale, gpointer user_data) {
|
|
||||||
gfloat cf = gtk_range_get_value(GTK_RANGE(scale));
|
|
||||||
gint c = round(cf);
|
|
||||||
g_message("foo %d %f", user_data, c);
|
|
||||||
IvySendMsg("ME RAW_DATALINK 16 SETTING;%d;0;%d", (gint)user_data, c);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
static void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
guint time_ticks = atoi(argv[0]);
|
|
||||||
double throttle = atof(argv[1]);
|
|
||||||
double rpm = atof(argv[2]);
|
|
||||||
double amp = atof(argv[3]);
|
|
||||||
double thrust = atof(argv[4]);
|
|
||||||
double torque = atof(argv[5]);
|
|
||||||
guint time_sec = atoi(argv[6]);
|
|
||||||
guint mode = atoi(argv[7]);
|
|
||||||
mb_state.mode = mode;
|
|
||||||
mb_state.time = (double)time_sec + (double)time_ticks/15000000.;
|
|
||||||
mb_state.throttle = throttle;
|
|
||||||
mb_state.rpms = rpm;
|
|
||||||
mb_state.amps = amp;
|
|
||||||
mb_state.thrust = thrust;
|
|
||||||
mb_state.torque = torque;
|
|
||||||
if (mb_state.log_channel) {
|
|
||||||
GString* str = g_string_sized_new(256);
|
|
||||||
g_string_printf(str, "%.4f %.3f %.0f %.1f %.1f %.1f\n", mb_state.time, mb_state.throttle, mb_state.rpms, mb_state.amps, mb_state.thrust, mb_state.torque);
|
|
||||||
gsize b_writen;
|
|
||||||
GError* my_err = NULL;
|
|
||||||
GIOStatus stat = g_io_channel_write_chars(mb_state.log_channel,str->str, str->len, &b_writen, &my_err);
|
|
||||||
g_string_free(str, TRUE);
|
|
||||||
}
|
|
||||||
// g_message("foo %f %f %f %f %d", mb_state.time, throttle, rpm, amp, mode);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void on_MOTOR_BENCH_STATIC(IvyClientPtr app, void *user_data, int argc, char *argv[]){
|
|
||||||
mb_state.av_rpm = atof(argv[0]);
|
|
||||||
mb_state.av_thrust = atof(argv[1]);
|
|
||||||
mb_state.av_amps = atof(argv[2]);
|
|
||||||
mb_state.av_throttle = atof(argv[3]);
|
|
||||||
|
|
||||||
if (mb_state.log_channel_static) {
|
|
||||||
GString* str = g_string_sized_new(256);
|
|
||||||
g_string_printf(str, "%0f %.3f %.2f %.1f\n", mb_state.av_throttle, mb_state.av_rpm, mb_state.av_amps, mb_state.av_thrust);
|
|
||||||
gsize b_writen;
|
|
||||||
GError* my_err = NULL;
|
|
||||||
GIOStatus stat = g_io_channel_write_chars(mb_state.log_channel_static,str->str, str->len, &b_writen, &my_err);
|
|
||||||
g_string_free(str, TRUE);
|
|
||||||
}
|
|
||||||
g_message("in_static %f %f %f %f", mb_state.av_throttle, mb_state.av_rpm, mb_state.av_amps, mb_state.av_thrust);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static gboolean timeout_callback(gpointer data) {
|
|
||||||
GString* str = g_string_sized_new(64);
|
|
||||||
g_string_printf(str, "%.2f s", mb_state.time);
|
|
||||||
gtk_label_set_text(GTK_LABEL(mb_gui.lab_time), str->str);
|
|
||||||
g_string_printf(str, "%.2f %%", mb_state.throttle*100.);
|
|
||||||
gtk_label_set_text(GTK_LABEL(mb_gui.lab_throttle), str->str);
|
|
||||||
g_string_printf(str, "%.0f rpms", mb_state.rpms);
|
|
||||||
gtk_label_set_text(GTK_LABEL(mb_gui.lab_rpms), str->str);
|
|
||||||
g_string_printf(str, "%.1f a", mb_state.amps);
|
|
||||||
gtk_label_set_text(GTK_LABEL(mb_gui.lab_amps), str->str);
|
|
||||||
g_string_printf(str, "%.1f g", mb_state.thrust);
|
|
||||||
gtk_label_set_text(GTK_LABEL(mb_gui.lab_thrust), str->str);
|
|
||||||
g_string_printf(str, "%.1f g", mb_state.torque);
|
|
||||||
gtk_label_set_text(GTK_LABEL(mb_gui.lab_torque), str->str);
|
|
||||||
g_string_free(str, TRUE);
|
|
||||||
return TRUE;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void on_log_button_toggled (GtkWidget *widget, gpointer data) {
|
|
||||||
if (gtk_toggle_button_get_active (GTK_TOGGLE_BUTTON (widget))) {
|
|
||||||
gtk_editable_set_editable( GTK_EDITABLE(mb_gui.entry_log), FALSE );
|
|
||||||
const gchar *log_file_name = gtk_entry_get_text (GTK_ENTRY (mb_gui.entry_log));
|
|
||||||
GError* my_err = NULL;
|
|
||||||
mb_state.log_channel = g_io_channel_new_file (log_file_name, "w", &my_err);
|
|
||||||
GString* static_name = g_string_sized_new(128);
|
|
||||||
g_string_printf(static_name,"%s%s", log_file_name, "_static");
|
|
||||||
mb_state.log_channel_static = g_io_channel_new_file (static_name->str, "w", &my_err);
|
|
||||||
g_string_free(static_name, TRUE);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
gtk_editable_set_editable( GTK_EDITABLE(mb_gui.entry_log), TRUE );
|
|
||||||
if (mb_state.log_channel) {
|
|
||||||
g_io_channel_close(mb_state.log_channel);
|
|
||||||
g_io_channel_close(mb_state.log_channel_static);
|
|
||||||
mb_state.log_channel = NULL;
|
|
||||||
mb_state.log_channel_static = NULL;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static void on_as_test_button_clicked (GtkWidget *widget, gpointer data) {
|
|
||||||
#ifdef ASTECH
|
|
||||||
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_TWI_CONTROLLER_ASCTECH_COMMAND_TYPE, AS_CMD_TEST_ADDR);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
static void on_as_reverse_button_clicked (GtkWidget *widget, gpointer data) {
|
|
||||||
#ifdef ASTECH
|
|
||||||
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_TWI_CONTROLLER_ASCTECH_COMMAND_TYPE, AS_CMD_REVERSE);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static void on_as_addr_changed (GtkRadioButton *radiobutton, gpointer user_data) {
|
|
||||||
#ifdef ASTECH
|
|
||||||
if (!gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)))
|
|
||||||
return;
|
|
||||||
guint new_addr = (guint)user_data;
|
|
||||||
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_TWI_CONTROLLER_ASCTECH_ADDR, new_addr);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int main (int argc, char** argv) {
|
|
||||||
|
|
||||||
gtk_init(&argc, &argv);
|
|
||||||
|
|
||||||
GtkWidget* window = build_gui();
|
|
||||||
gtk_widget_show_all(window);
|
|
||||||
|
|
||||||
IvyInit ("MotorBench", "MotorBench READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
|
||||||
IvyBindMsg(on_MOTOR_BENCH_STATIC, NULL, "^\\S* MOTOR_BENCH_STATIC (\\S*) (\\S*) (\\S*) (\\S*)");
|
|
||||||
IvyStart("127.255.255.255");
|
|
||||||
|
|
||||||
g_timeout_add(40, timeout_callback, NULL);
|
|
||||||
|
|
||||||
mb_state.log_channel = NULL;
|
|
||||||
|
|
||||||
gtk_main();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static GtkWidget* build_gui ( void ) {
|
|
||||||
|
|
||||||
GtkWidget *window1;
|
|
||||||
GtkWidget *vbox1;
|
|
||||||
|
|
||||||
|
|
||||||
window1 = gtk_window_new (GTK_WINDOW_TOPLEVEL);
|
|
||||||
gtk_window_set_title (GTK_WINDOW (window1), "motor_bench");
|
|
||||||
|
|
||||||
vbox1 = gtk_vbox_new (FALSE, 0);
|
|
||||||
gtk_container_add (GTK_CONTAINER (window1), vbox1);
|
|
||||||
|
|
||||||
GtkWidget* hbox1 = gtk_hbox_new (FALSE, 0);
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox1), hbox1, TRUE, TRUE, 0);
|
|
||||||
|
|
||||||
//
|
|
||||||
// Modes
|
|
||||||
//
|
|
||||||
GtkWidget *mode_frame = gtk_frame_new ("Mode");
|
|
||||||
gtk_container_set_border_width (GTK_CONTAINER (mode_frame), 10);
|
|
||||||
gtk_box_pack_start (GTK_BOX (hbox1), mode_frame, TRUE, TRUE, 0);
|
|
||||||
|
|
||||||
GtkWidget* vbox2 = gtk_vbox_new (FALSE, 0);
|
|
||||||
gtk_container_add (GTK_CONTAINER (mode_frame), vbox2);
|
|
||||||
|
|
||||||
GSList *rb_mode_group = NULL;
|
|
||||||
GtkWidget* rb_idle = gtk_radio_button_new_with_mnemonic (NULL, "idle");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_idle, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_idle), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_idle));
|
|
||||||
GtkWidget* rb_manual = gtk_radio_button_new_with_mnemonic (NULL, "manual");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_manual, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_manual), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_manual));
|
|
||||||
GtkWidget* rb_ramp = gtk_radio_button_new_with_mnemonic (NULL, "ramp");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_ramp, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_ramp), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_ramp));
|
|
||||||
GtkWidget* rb_step = gtk_radio_button_new_with_mnemonic (NULL, "step");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_step, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_step), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_step));
|
|
||||||
GtkWidget* rb_prbs = gtk_radio_button_new_with_mnemonic (NULL, "prbs");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_prbs, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_prbs), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_prbs));
|
|
||||||
GtkWidget* rb_sine = gtk_radio_button_new_with_mnemonic (NULL, "sine");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_sine, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_sine), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_sine));
|
|
||||||
GtkWidget* rb_fixed_rpm = gtk_radio_button_new_with_mnemonic (NULL, "fixed");
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox2), rb_fixed_rpm, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_fixed_rpm), rb_mode_group);
|
|
||||||
rb_mode_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_fixed_rpm));
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
g_signal_connect ((gpointer) rb_idle, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_IDLE);
|
|
||||||
g_signal_connect ((gpointer) rb_manual, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_MANUAL);
|
|
||||||
g_signal_connect ((gpointer) rb_ramp, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_RAMP);
|
|
||||||
g_signal_connect ((gpointer) rb_step, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_STEP);
|
|
||||||
g_signal_connect ((gpointer) rb_prbs, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_PRBS);
|
|
||||||
g_signal_connect ((gpointer) rb_sine, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_SINE);
|
|
||||||
g_signal_connect ((gpointer) rb_fixed_rpm, "toggled", G_CALLBACK (on_mode_changed), (gpointer)MB_MODES_FIXED_RPM);
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// Measures
|
|
||||||
//
|
|
||||||
GtkWidget *measure_frame = gtk_frame_new ("Measures");
|
|
||||||
gtk_container_set_border_width (GTK_CONTAINER (measure_frame), 10);
|
|
||||||
gtk_box_pack_start (GTK_BOX (hbox1), measure_frame, TRUE, TRUE, 0);
|
|
||||||
|
|
||||||
GtkWidget* table1 = gtk_table_new (2, 3, FALSE);
|
|
||||||
gtk_container_add (GTK_CONTAINER (measure_frame), table1);
|
|
||||||
gtk_table_set_col_spacings (GTK_TABLE (table1), 5);
|
|
||||||
|
|
||||||
GtkWidget* t_time = gtk_label_new ("time");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), t_time, 0, 1, 0, 1,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (t_time), 0, 0.5);
|
|
||||||
mb_gui.lab_time = gtk_label_new ("XXXX");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), mb_gui.lab_time, 1, 2, 0, 1,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (mb_gui.lab_time), 0, 0.5);
|
|
||||||
|
|
||||||
GtkWidget* t_throttle = gtk_label_new ("throttle");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), t_throttle, 0, 1, 1, 2,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (t_throttle), 0, 0.5);
|
|
||||||
mb_gui.lab_throttle = gtk_label_new ("XXXX");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), mb_gui.lab_throttle, 1, 2, 1, 2,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (mb_gui.lab_throttle), 0, 0.5);
|
|
||||||
|
|
||||||
GtkWidget* t_rpms = gtk_label_new ("rpms");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), t_rpms, 0, 1, 2, 3,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (t_rpms), 0, 0.5);
|
|
||||||
mb_gui.lab_rpms = gtk_label_new ("XXXX");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), mb_gui.lab_rpms, 1, 2, 2, 3,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (mb_gui.lab_rpms), 0, 0.5);
|
|
||||||
|
|
||||||
GtkWidget* t_amps = gtk_label_new ("amps");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), t_amps, 0, 1, 3, 4,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (t_amps), 0, 0.5);
|
|
||||||
mb_gui.lab_amps = gtk_label_new ("XXXX");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), mb_gui.lab_amps, 1, 2, 3, 4,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (mb_gui.lab_amps), 0, 0.5);
|
|
||||||
|
|
||||||
GtkWidget* t_thrust = gtk_label_new ("Thrust");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), t_thrust, 0, 1, 4, 5,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (t_thrust), 0, 0.5);
|
|
||||||
mb_gui.lab_thrust = gtk_label_new ("XXXX");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), mb_gui.lab_thrust, 1, 2, 4, 5,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (mb_gui.lab_thrust), 0, 0.5);
|
|
||||||
|
|
||||||
GtkWidget* t_torque = gtk_label_new ("Torque");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), t_torque, 0, 1, 5, 6,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (t_torque), 0, 0.5);
|
|
||||||
mb_gui.lab_torque = gtk_label_new ("XXXX");
|
|
||||||
gtk_table_attach (GTK_TABLE (table1), mb_gui.lab_torque, 1, 2, 5, 6,
|
|
||||||
(GtkAttachOptions) (GTK_FILL),
|
|
||||||
(GtkAttachOptions) (0), 0, 0);
|
|
||||||
gtk_misc_set_alignment (GTK_MISC (mb_gui.lab_torque), 0, 0.5);
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// Log
|
|
||||||
//
|
|
||||||
GtkWidget *log_frame = gtk_frame_new ("Log");
|
|
||||||
gtk_container_set_border_width (GTK_CONTAINER (log_frame), 10);
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox1), log_frame, TRUE, TRUE, 0);
|
|
||||||
|
|
||||||
|
|
||||||
GtkWidget* bbox = gtk_hbutton_box_new ();
|
|
||||||
gtk_container_add (GTK_CONTAINER (log_frame), bbox);
|
|
||||||
|
|
||||||
GtkWidget* log_button = gtk_toggle_button_new_with_label( "Log" );
|
|
||||||
gtk_container_add (GTK_CONTAINER (bbox), log_button);
|
|
||||||
g_signal_connect (G_OBJECT (log_button), "toggled", G_CALLBACK (on_log_button_toggled), NULL);
|
|
||||||
|
|
||||||
mb_gui.entry_log = gtk_entry_new( );
|
|
||||||
gtk_container_add (GTK_CONTAINER (bbox), mb_gui.entry_log);
|
|
||||||
gtk_entry_set_text( GTK_ENTRY(mb_gui.entry_log), "mb_log.txt" );
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// Asctech
|
|
||||||
//
|
|
||||||
|
|
||||||
GtkWidget *asctech_frame = gtk_frame_new ("Asctech");
|
|
||||||
gtk_container_set_border_width (GTK_CONTAINER (asctech_frame), 10);
|
|
||||||
gtk_box_pack_start (GTK_BOX (vbox1), asctech_frame, TRUE, TRUE, 0);
|
|
||||||
|
|
||||||
|
|
||||||
GtkWidget* as_vbox2 = gtk_vbox_new (FALSE, 0);
|
|
||||||
gtk_container_add (GTK_CONTAINER (asctech_frame), as_vbox2);
|
|
||||||
|
|
||||||
GSList *rb_addr_group = NULL;
|
|
||||||
GtkWidget* rb_front = gtk_radio_button_new_with_mnemonic (NULL, "front");
|
|
||||||
gtk_box_pack_start (GTK_BOX (as_vbox2), rb_front, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_front), rb_addr_group);
|
|
||||||
rb_addr_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_front));
|
|
||||||
GtkWidget* rb_back = gtk_radio_button_new_with_mnemonic (NULL, "back");
|
|
||||||
gtk_box_pack_start (GTK_BOX (as_vbox2), rb_back, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_back), rb_addr_group);
|
|
||||||
rb_addr_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_back));
|
|
||||||
GtkWidget* rb_left = gtk_radio_button_new_with_mnemonic (NULL, "left");
|
|
||||||
gtk_box_pack_start (GTK_BOX (as_vbox2), rb_left, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_left), rb_addr_group);
|
|
||||||
rb_addr_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_left));
|
|
||||||
GtkWidget* rb_right = gtk_radio_button_new_with_mnemonic (NULL, "right");
|
|
||||||
gtk_box_pack_start (GTK_BOX (as_vbox2), rb_right, TRUE, TRUE, 0);
|
|
||||||
gtk_radio_button_set_group (GTK_RADIO_BUTTON (rb_right), rb_addr_group);
|
|
||||||
rb_addr_group = gtk_radio_button_get_group (GTK_RADIO_BUTTON (rb_right));
|
|
||||||
|
|
||||||
g_signal_connect ((gpointer) rb_front, "toggled", G_CALLBACK (on_as_addr_changed), (gpointer)AS_MOT_FRONT);
|
|
||||||
g_signal_connect ((gpointer) rb_back, "toggled", G_CALLBACK (on_as_addr_changed), (gpointer)AS_MOT_BACK);
|
|
||||||
g_signal_connect ((gpointer) rb_left, "toggled", G_CALLBACK (on_as_addr_changed), (gpointer)AS_MOT_LEFT);
|
|
||||||
g_signal_connect ((gpointer) rb_right, "toggled", G_CALLBACK (on_as_addr_changed), (gpointer)AS_MOT_RIGHT);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
GtkWidget* as_bbox = gtk_hbutton_box_new ();
|
|
||||||
gtk_box_pack_start (GTK_BOX (as_vbox2), as_bbox, TRUE, TRUE, 0);
|
|
||||||
//gtk_container_add (GTK_CONTAINER (asctech_frame), as_bbox);
|
|
||||||
|
|
||||||
GtkWidget* test_button = gtk_toggle_button_new_with_label( "Test" );
|
|
||||||
gtk_container_add (GTK_CONTAINER (as_bbox), test_button);
|
|
||||||
g_signal_connect (G_OBJECT (test_button), "clicked", G_CALLBACK (on_as_test_button_clicked), NULL);
|
|
||||||
|
|
||||||
GtkWidget* reverse_button = gtk_toggle_button_new_with_label( "Reverse" );
|
|
||||||
gtk_container_add (GTK_CONTAINER (as_bbox), reverse_button);
|
|
||||||
g_signal_connect (G_OBJECT (reverse_button), "clicked", G_CALLBACK (on_as_reverse_button_clicked), NULL);
|
|
||||||
|
|
||||||
|
|
||||||
return window1;
|
|
||||||
}
|
|
||||||
@@ -1,144 +0,0 @@
|
|||||||
|
|
||||||
|
|
||||||
function [time, throttle, rpm, amp, thrust, torque] = read_mb_log(filename)
|
|
||||||
|
|
||||||
time=[];
|
|
||||||
throttle=[];
|
|
||||||
rpm=[];
|
|
||||||
amp=[];
|
|
||||||
thrust=[];
|
|
||||||
torque=[];
|
|
||||||
|
|
||||||
u=mopen(filename,'r');
|
|
||||||
|
|
||||||
while meof(u) == 0,
|
|
||||||
line = mgetl(u, 1);
|
|
||||||
if strindex(line, '#') ~= 1 & length(line) ~= 0,
|
|
||||||
[nb_scan, _time, _throttle, _rpm, _amp, _thrust, _torque] = msscanf(1, line, '%f %f %f %f %f %f');
|
|
||||||
if nb_scan == 6,
|
|
||||||
time = [time _time];
|
|
||||||
throttle = [throttle _throttle];
|
|
||||||
rpm = [rpm _rpm];
|
|
||||||
amp = [amp _amp];
|
|
||||||
thrust = [thrust _thrust];
|
|
||||||
torque = [torque _torque];
|
|
||||||
end
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
mclose(u);
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
function [averaged] = average(nb_points, raw_data_throttle, raw_data_rpm)
|
|
||||||
av_rpm=zeros(1,nb_points+1);
|
|
||||||
count=zeros(1,nb_points+1);
|
|
||||||
//Summation of the Data
|
|
||||||
for i=1:length(raw_data_throttle)
|
|
||||||
idx=round(raw_data_throttle(i)*nb_points)+1;
|
|
||||||
av_rpm(idx) = av_rpm(idx) + raw_data_rpm(i);
|
|
||||||
count(idx) = count(idx) + 1;
|
|
||||||
end
|
|
||||||
|
|
||||||
// Dividing the Data by the summated points
|
|
||||||
|
|
||||||
for i=1:nb_points+1
|
|
||||||
if count(i) ~=0
|
|
||||||
av_rpm(i) = av_rpm(i) / count(i);
|
|
||||||
// printf('i=%d count=%d av_rpm=%d\n', i, count(i), av_rpm(i) );
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
averaged = av_rpm;
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
function [filtered] = low_pass_filter(f_sample, f_cut, raw_data)
|
|
||||||
|
|
||||||
delta_t = 1/f_sample;
|
|
||||||
rc = 1 / ( 2 * %pi * f_cut);
|
|
||||||
alpha = delta_t / ( delta_t + rc );
|
|
||||||
|
|
||||||
filtered=[raw_data(1)];
|
|
||||||
for i=2:length(raw_data)
|
|
||||||
fv = alpha * raw_data(i) + (1 - alpha) * filtered(i-1);
|
|
||||||
filtered = [filtered fv];
|
|
||||||
end
|
|
||||||
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// Definition of the Fittin Function
|
|
||||||
// FF=Fitting Function
|
|
||||||
// x=Vector of Variables
|
|
||||||
// p=Vector of Parameters
|
|
||||||
function y=FF(x,p)
|
|
||||||
y=p(1)+((sqrt(p(2)*x) +1)-1)/p(3)
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
//The criterion function
|
|
||||||
//Discrete errorfunction
|
|
||||||
//z = x Vector and y Vector of the collected Data
|
|
||||||
function e=G(p,z),
|
|
||||||
y=z(1),x=z(2);
|
|
||||||
e=y-FF(x,p),
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
function [] = param_fit(av_rpm, av_throttle)
|
|
||||||
|
|
||||||
X=av_throttle;
|
|
||||||
Y=av_rpm;
|
|
||||||
Z=[X;Y];
|
|
||||||
//solve the Problem
|
|
||||||
//Initial parameters
|
|
||||||
p0 = [1;1;1];
|
|
||||||
//call the datafit function with following parameter:
|
|
||||||
//G = errorfunction
|
|
||||||
//Z = collected Data Vector
|
|
||||||
//p0= initial parameter
|
|
||||||
[p,err] = datafit(G,Z,p0);
|
|
||||||
|
|
||||||
printf('\n')
|
|
||||||
printf('p=%f\n', p)
|
|
||||||
|
|
||||||
scf(0);clf()
|
|
||||||
//plot2d(X,FF(X,pg),5) //the curve without noise
|
|
||||||
subplot(2,1,1);
|
|
||||||
plot2d(X,Y,1) // the noisy data
|
|
||||||
subplot(2,1,2);
|
|
||||||
plot2d(X,FF(X,p),12) //the solution
|
|
||||||
param_fitted=FF(X,p);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
function r = mot_lin(t, p)
|
|
||||||
r = p(1) * t + p(2);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
function e = err_mot_lin(p, z)
|
|
||||||
r=z(1), t=z(2);
|
|
||||||
e = r - mot_lin(t, p);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
function rpm = mot_ric_stat(throttle,param)
|
|
||||||
// maybe 1 +
|
|
||||||
tau_kq = param(1);
|
|
||||||
kv = param(2);
|
|
||||||
dv = 0.0;
|
|
||||||
rpm = (-1 + sqrt(1 + 4 * tau_kq * kv * (throttle + dv))) / (2 * tau_kq);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
function e = err_mot_ric(p, z)
|
|
||||||
r_meas = z(1);
|
|
||||||
throttle = z(2);
|
|
||||||
e = r_meas - mot_ric_stat(throttle, p);
|
|
||||||
endfunction
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
function rpm_dot = mot_ode(tau, kq, kv, V, rpm)
|
|
||||||
rpm_dot = -1/tau*rpm - kq * rpm^2 + kv/tau*(V);
|
|
||||||
endfunction
|
|
||||||
@@ -1,29 +0,0 @@
|
|||||||
clear();
|
|
||||||
getf('mb_utils.sci');
|
|
||||||
|
|
||||||
load('averaged_ramp_geared.dat','av_throttle','av_rpm')
|
|
||||||
|
|
||||||
t_part = av_throttle(20:90);
|
|
||||||
r_part = av_rpm(20:90);
|
|
||||||
|
|
||||||
p0=[0.00001; 20000];
|
|
||||||
Z = [r_part; t_part];
|
|
||||||
//[p, err] = datafit(err_mot_lin, Z, p0);
|
|
||||||
|
|
||||||
[p, err] = datafit(err_mot_ric, Z, p0)
|
|
||||||
//p = p0;
|
|
||||||
|
|
||||||
xbasc();
|
|
||||||
//subplot(3,1,1);
|
|
||||||
xtitle('Rpms vs Throttle');
|
|
||||||
plot2d(av_throttle,av_rpm);
|
|
||||||
plot2d(t_part,r_part, 4);
|
|
||||||
|
|
||||||
|
|
||||||
r_fit = [];
|
|
||||||
for i=1:length(av_throttle)
|
|
||||||
// r_fit = [r_fit mot_lin(av_throttle(i), p)];
|
|
||||||
r_fit = [r_fit mot_ric(av_throttle(i), p)];
|
|
||||||
end
|
|
||||||
|
|
||||||
plot2d(av_throttle,r_fit, 3);
|
|
||||||
@@ -1,32 +0,0 @@
|
|||||||
clear();
|
|
||||||
getf('mb_utils.sci');
|
|
||||||
|
|
||||||
//filename = "mb_log.txt";
|
|
||||||
args = sciargs();
|
|
||||||
[nb_args, foo] = size(args)
|
|
||||||
filename = args(nb_args);
|
|
||||||
filename = "asctech/log_ramp_crooked_prop.txt"
|
|
||||||
|
|
||||||
[time, throttle, rpm, amp, thrust, torque] = read_mb_log(filename);
|
|
||||||
|
|
||||||
|
|
||||||
f_sample = 250.;
|
|
||||||
fc = 100.;
|
|
||||||
f_rpm = low_pass_filter(f_sample, fc, rpm);
|
|
||||||
|
|
||||||
|
|
||||||
xbasc();
|
|
||||||
subplot(3,1,1)
|
|
||||||
xtitle('Throttle');
|
|
||||||
plot2d(time, throttle);
|
|
||||||
|
|
||||||
subplot(3,1,2)
|
|
||||||
xtitle('Rpm');
|
|
||||||
plot2d(time, rpm);
|
|
||||||
|
|
||||||
subplot(3,1,3)
|
|
||||||
xtitle('Filtered Rpm');
|
|
||||||
plot2d(time, f_rpm);
|
|
||||||
//plot2d(rpm, throttle);
|
|
||||||
|
|
||||||
save('asctech/log_ramp_crooked_prop.dat', time, throttle, rpm, amp, thrust, torque);
|
|
||||||
@@ -1,73 +0,0 @@
|
|||||||
--------
|
|
||||||
|BUILDING|
|
|
||||||
--------
|
|
||||||
Make sure you have the environment variable RTHOME set to the absolute
|
|
||||||
path to the river_tracking directory on your system. (Place this in
|
|
||||||
your .bashrc or environment variable settings in your IDE).
|
|
||||||
|
|
||||||
Build with make.
|
|
||||||
|
|
||||||
|
|
||||||
--------------------
|
|
||||||
|USING WITH PAPARAZZI|
|
|
||||||
--------------------
|
|
||||||
In order for river_tracking to function properly with Paparazzi, you must
|
|
||||||
create the flight plan with the following specifications:
|
|
||||||
|
|
||||||
Waypoints:
|
|
||||||
----------
|
|
||||||
-Your waypoints must be set up something like this:
|
|
||||||
<waypoints>
|
|
||||||
<waypoint alt="xxx" name="HOME" x="xxx" y="xxx"/>
|
|
||||||
<waypoint name="pre_track_setup" x="xxx" y="xxx"/>
|
|
||||||
<waypoint name="pre_track_start" x="xxx" y="xxx"/>
|
|
||||||
<waypoint name="tracker" x="xxx" y="xxx"/>
|
|
||||||
...other stuff...
|
|
||||||
</waypoints>
|
|
||||||
|
|
||||||
Explanation:
|
|
||||||
-The first waypoint defined can be anything (something like HOME is good)
|
|
||||||
-The second and third waypoints defined must be set up such that a line which
|
|
||||||
passes through them will also pass through the first point on the river to be
|
|
||||||
tracked. It is recommended that they be spaced at least 100ish meters apart
|
|
||||||
to ensure that the plane will indeed be flying in a straight line once
|
|
||||||
it reaches the first point on the river
|
|
||||||
-The fourth waypoint defined is the waypoint which river_tracking will move
|
|
||||||
around. It must be placed initially on the river at the desired starting point.
|
|
||||||
-All other waypoints can be set up however you'd like.
|
|
||||||
|
|
||||||
Blocks:
|
|
||||||
-------
|
|
||||||
-Your blocks must look something like this:
|
|
||||||
<blocks>
|
|
||||||
<block name="pre_track">
|
|
||||||
<go wp="pre_track_setup"/>
|
|
||||||
<go from="pre_track_setup" wp="pre_track_start" hmode="route"/>
|
|
||||||
</block>
|
|
||||||
<block name="track">
|
|
||||||
<while cond="TRUE">
|
|
||||||
<go wp="tracker"/>
|
|
||||||
</while>
|
|
||||||
</block>
|
|
||||||
...other stuff...
|
|
||||||
</blocks>
|
|
||||||
|
|
||||||
Explanation:
|
|
||||||
-The first block defined must go through the second and third waypoints.
|
|
||||||
-The second block defined must continually go to the fourth waypoint.
|
|
||||||
-All other blocks can be set up however you'd like.
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
-----------
|
|
||||||
|DEVELOPMENT|
|
|
||||||
-----------
|
|
||||||
|
|
||||||
Project Layout:
|
|
||||||
---------------
|
|
||||||
All source files (.c, .h, etc) should be placed under the src directory.
|
|
||||||
The river_track executable (and other executables) will be placed in the bin
|
|
||||||
directory.
|
|
||||||
@@ -1,24 +0,0 @@
|
|||||||
#Makefile written by Mitchel Humpherys
|
|
||||||
|
|
||||||
#if pkg-config --cflags opencv --libs opencv
|
|
||||||
#doesn't return something useful
|
|
||||||
#make sure PKG_CONFIG_PATH is set correctly
|
|
||||||
#Also, RTHOME must be set to the absolute path of the river_tracking
|
|
||||||
#directory in your environment.
|
|
||||||
RTSOURCE = $(RTHOME)/src
|
|
||||||
RTBIN = $(RTHOME)/bin
|
|
||||||
INCLUDES = `pkg-config --cflags --libs opencv gtk+-2.0` -I$(RTSOURCE) -lglibivy
|
|
||||||
|
|
||||||
CPP = g++ -g -Wno-deprecated
|
|
||||||
CC = gcc -g
|
|
||||||
CFLAGS = -c $(INCLUDES)
|
|
||||||
|
|
||||||
all : main
|
|
||||||
|
|
||||||
main : src/river_track.c
|
|
||||||
$(CC) src/river_track.c -o bin/river_track $(INCLUDES)
|
|
||||||
|
|
||||||
clean :
|
|
||||||
cd $(RTHOME) rm -f *.o
|
|
||||||
cd $(RTSOURCE); rm -f *.o;
|
|
||||||
cd $(RTBIN); rm -f river_track;
|
|
||||||
@@ -1,49 +0,0 @@
|
|||||||
#include <stdlib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <getopt.h>
|
|
||||||
#include <ivy.h>
|
|
||||||
#include <ivyloop.h>
|
|
||||||
|
|
||||||
/* callback associated to "Hello" messages */
|
|
||||||
void HelloCallback (IvyClientPtr app, void *data, int argc, char **argv)
|
|
||||||
{
|
|
||||||
const char* arg = (argc < 1) ? "" : argv[0];
|
|
||||||
IvySendMsg ("Bonjour%s", arg);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* callback associated to "Bye" messages */
|
|
||||||
void ByeCallback (IvyClientPtr app, void *data, int argc, char **argv)
|
|
||||||
{
|
|
||||||
IvyStop ();
|
|
||||||
}
|
|
||||||
|
|
||||||
main (int argc, char**argv)
|
|
||||||
{
|
|
||||||
/* handling of -b option */
|
|
||||||
const char* bus = 0;
|
|
||||||
char c;
|
|
||||||
while (c = getopt (argc, argv, "b:") != EOF) {
|
|
||||||
switch (c) {
|
|
||||||
case 'b':
|
|
||||||
bus = optarg;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* handling of environment variable */
|
|
||||||
if (!bus)
|
|
||||||
bus = getenv ("IVYBUS");
|
|
||||||
|
|
||||||
/* initializations */
|
|
||||||
IvyInit ("IvyTranslater", "Hello le monde", 0, 0, 0, 0);
|
|
||||||
IvyStart (bus);
|
|
||||||
|
|
||||||
/* binding of HelloCallback to messages starting with 'Hello' */
|
|
||||||
IvyBindMsg (HelloCallback, 0, "^Hello(.*)");
|
|
||||||
|
|
||||||
/* binding of ByeCallback to 'Bye' */
|
|
||||||
IvyBindMsg (ByeCallback, 0, "^Bye$");
|
|
||||||
|
|
||||||
/* main loop */
|
|
||||||
IvyMainLoop(0);
|
|
||||||
}
|
|
||||||
@@ -1,20 +0,0 @@
|
|||||||
/*This module will do a bunch of fancy opencv stuff to figure out the
|
|
||||||
next waypoint.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "waypoint.h"
|
|
||||||
|
|
||||||
extern float ADD;
|
|
||||||
|
|
||||||
Waypoint get_next_waypoint() {
|
|
||||||
Waypoint next_wp;
|
|
||||||
|
|
||||||
/* Insert all sorts of opencv magic here */
|
|
||||||
next_wp.ac_id = 1;
|
|
||||||
next_wp.wp = 3;
|
|
||||||
next_wp.alt = 1420.;
|
|
||||||
next_wp.lat = 41.823364 + ADD;
|
|
||||||
next_wp.lon = -111.988800 + ADD;
|
|
||||||
ADD+=.00004;
|
|
||||||
return next_wp;
|
|
||||||
}
|
|
||||||
@@ -1,131 +0,0 @@
|
|||||||
/*
|
|
||||||
* File: river_track.c
|
|
||||||
* Author: mgalgs
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
#include <gtk-2.0/gtk/gtk.h>
|
|
||||||
/*#include <gtk/gtk.h> is what the previous line should read when compiling
|
|
||||||
with pkg-config --cflags --libs gtk+-2.0 */
|
|
||||||
#include "nextwp.c"
|
|
||||||
#include "waypoint.h"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* Some global variables */
|
|
||||||
float END_LAT = 0.0; //latitude of the "end" waypoint (the fifth waypoint defined in flight plan)
|
|
||||||
float END_LON = 0.0; //longitude of the "end" waypoint (the fifth waypoint defined in flight plan)
|
|
||||||
int CONTINUE = 1; //When this variable is set to 0 river_track will stop moving the waypoint around
|
|
||||||
float ADD = 0; //just to move the waypoint around for fun
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* callback associated to "Hello" messages */
|
|
||||||
void textCallback(IvyClientPtr app, void *data, int argc, char **argv)
|
|
||||||
{
|
|
||||||
const char* arg = (argc < 1) ? "" : argv[0];
|
|
||||||
IvySendMsg("Alo ai%s", arg);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/* Sets the global variables END_LAT & END_LON to the GPS coordinates
|
|
||||||
* of the end waypoint (the fifth waypoint defined in the flight plan).
|
|
||||||
*/
|
|
||||||
void set_end(IvyClientPtr app, void *data, int argc, char **argv)
|
|
||||||
{
|
|
||||||
const char *arg = (argc < 1) ? "" : argv[0];
|
|
||||||
|
|
||||||
char *current;
|
|
||||||
current = strtok(arg, " ");
|
|
||||||
current = strtok(NULL, " ");
|
|
||||||
current = strtok(NULL, " ");
|
|
||||||
current = strtok(NULL, " ");
|
|
||||||
END_LAT = atof(current);
|
|
||||||
current = strtok(NULL, " ");
|
|
||||||
END_LON = atof(current);
|
|
||||||
// fprintf(stderr, "END_LAT=%f. END_LON=%f.", END_LAT, END_LON);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Called by clicking a button */
|
|
||||||
void init_river_tracking( GtkWidget *widget, gpointer data )
|
|
||||||
{
|
|
||||||
fprintf(stderr, "initializing river tracking...\n");
|
|
||||||
/* Call function get_next_waypoint (defined in nexcwp.c)
|
|
||||||
* which returns a waypoint. */
|
|
||||||
Waypoint new_wp = get_next_waypoint();
|
|
||||||
IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
|
|
||||||
new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/* Called when aircraft reaches "Block 1" (the second
|
|
||||||
* block defined in the flight plan)
|
|
||||||
*/
|
|
||||||
void start_track(IvyClientPtr app, void *data, int argc, char **argv)
|
|
||||||
{
|
|
||||||
fprintf(stderr, ".");
|
|
||||||
/* Call function get_next_waypoint (defined in nexcwp.c)
|
|
||||||
* which returns a waypoint. */
|
|
||||||
Waypoint new_wp = get_next_waypoint();
|
|
||||||
|
|
||||||
if(CONTINUE) {
|
|
||||||
IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
|
|
||||||
new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt); //Always move waypoint 4
|
|
||||||
}
|
|
||||||
/* else we've reached the end (we've seen the "end" waypoint in the
|
|
||||||
* picture), so don't send any message.
|
|
||||||
*/
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/* Destroys the window */
|
|
||||||
static void destroy( GtkWidget *widget, gpointer data )
|
|
||||||
{
|
|
||||||
gtk_main_quit ();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main( int argc, char *argv[] )
|
|
||||||
{
|
|
||||||
GtkWidget *window;
|
|
||||||
GtkWidget *button;
|
|
||||||
GtkWidget *box1;
|
|
||||||
|
|
||||||
char *bus=getenv("IVYBUS");
|
|
||||||
|
|
||||||
gtk_init(&argc, &argv);
|
|
||||||
window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
|
|
||||||
gtk_container_set_border_width(GTK_CONTAINER(window), 10);
|
|
||||||
gtk_window_set_title (GTK_WINDOW (window), "River Tracking");
|
|
||||||
|
|
||||||
box1 = gtk_hbox_new (FALSE, 0);
|
|
||||||
gtk_container_add (GTK_CONTAINER (window), box1);
|
|
||||||
|
|
||||||
//first button...
|
|
||||||
button = gtk_button_new_with_label("(Re)define region of interest");
|
|
||||||
g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(init_river_tracking), 0);
|
|
||||||
gtk_box_pack_start(GTK_BOX(box1), button, TRUE, TRUE, 0);
|
|
||||||
gtk_widget_show (button);
|
|
||||||
|
|
||||||
//second button...
|
|
||||||
button = gtk_button_new_with_label("Quit");
|
|
||||||
g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(destroy), 0);
|
|
||||||
gtk_box_pack_start(GTK_BOX (box1), button, TRUE, TRUE, 0);
|
|
||||||
gtk_widget_show (button);
|
|
||||||
|
|
||||||
gtk_widget_show(box1);
|
|
||||||
gtk_widget_show(window);
|
|
||||||
|
|
||||||
IvyInit("river_track", "river_track READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyBindMsg(textCallback,0,"^river_track hello(.*)");
|
|
||||||
IvyBindMsg(start_track,0,"(NAV_STATUS 1 1 +.*)");
|
|
||||||
IvyBindMsg(set_end,0,"(WAYPOINT_MOVED 1 5 +.*)");
|
|
||||||
IvyStart(bus);
|
|
||||||
|
|
||||||
gtk_main();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,20 +0,0 @@
|
|||||||
/*
|
|
||||||
* File: waypoint.h
|
|
||||||
* Author: mgalgs
|
|
||||||
*
|
|
||||||
* Created on July 28, 2008, 5:22 PM
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef _WAYPOINT_H
|
|
||||||
#define _WAYPOINT_H
|
|
||||||
|
|
||||||
typedef struct Waypoint {
|
|
||||||
int ac_id;
|
|
||||||
int wp;
|
|
||||||
float lat;
|
|
||||||
float lon;
|
|
||||||
float alt;
|
|
||||||
} Waypoint;
|
|
||||||
|
|
||||||
#endif /* _WAYPOINT_H */
|
|
||||||
|
|
||||||
@@ -1,14 +0,0 @@
|
|||||||
|
|
||||||
# Quiet compilation
|
|
||||||
Q=@
|
|
||||||
|
|
||||||
CC = gcc
|
|
||||||
GLIB_CFLAGS = -Wall $(shell pkg-config glib-2.0 --cflags)
|
|
||||||
GLIB_LDFLAGS = $(shell pkg-config glib-2.0 --libs) -lglibivy $(shell pcre-config --libs)
|
|
||||||
|
|
||||||
main: main.c serial_port.c
|
|
||||||
$(CC) $(GLIB_CFLAGS) -o $@ main.c serial_port.c $(GLIB_LDFLAGS)
|
|
||||||
|
|
||||||
|
|
||||||
clean:
|
|
||||||
$(Q)rm -f main
|
|
||||||
@@ -1,128 +0,0 @@
|
|||||||
#include <glib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
|
|
||||||
|
|
||||||
#include <Ivy/ivy.h>
|
|
||||||
#include <Ivy/ivyglibloop.h>
|
|
||||||
|
|
||||||
#include "serial_port.h"
|
|
||||||
|
|
||||||
#define BUF_SIZE 512
|
|
||||||
#define AC_ID 2
|
|
||||||
|
|
||||||
static struct SerialPort* sp;
|
|
||||||
static GIOChannel* ioc;
|
|
||||||
|
|
||||||
static void configure_term(struct termios *termios, speed_t *speed) {
|
|
||||||
/* input modes */
|
|
||||||
termios->c_iflag &= ~(IGNBRK|BRKINT|PARMRK|INPCK|ISTRIP|INLCR|IGNCR
|
|
||||||
|ICRNL |IUCLC|IXON|IXANY|IXOFF|IMAXBEL);
|
|
||||||
termios->c_iflag |= IGNPAR;
|
|
||||||
/* control modes*/
|
|
||||||
termios->c_cflag &= ~(CSIZE|PARENB|CRTSCTS|PARODD|HUPCL);
|
|
||||||
termios->c_cflag |= CREAD|CS8|CSTOPB|CLOCAL;
|
|
||||||
/* local modes */
|
|
||||||
termios->c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN);
|
|
||||||
termios->c_lflag |= NOFLSH;
|
|
||||||
/* speed */
|
|
||||||
*speed = B9600;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void dump_buf ( int len, char* buf) {
|
|
||||||
|
|
||||||
static gchar buf2[BUF_SIZE];
|
|
||||||
static int cur_len = 0;
|
|
||||||
int i;
|
|
||||||
|
|
||||||
for (i=0; i<len; i++) {
|
|
||||||
if (buf[i] != '\r') {
|
|
||||||
if (buf[i] != 'T' && buf[i] != '.'){
|
|
||||||
buf2[cur_len] = buf[i];
|
|
||||||
cur_len++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
int force;
|
|
||||||
buf2[cur_len] = 0;
|
|
||||||
g_message("buf2: %s",buf2);
|
|
||||||
sscanf(buf2,"%d",&force);
|
|
||||||
IvySendMsg("%d X_FORCE %d", AC_ID, force);
|
|
||||||
cur_len = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
//g_message("read %s and %s",buf,buf2);
|
|
||||||
}
|
|
||||||
|
|
||||||
static gboolean on_serial_data_received(GIOChannel *source,
|
|
||||||
GIOCondition condition,
|
|
||||||
gpointer data) {
|
|
||||||
gchar* _buf;
|
|
||||||
GError* _err = NULL;
|
|
||||||
GIOStatus st = g_io_channel_read_line(source, &_buf, NULL, NULL, &_err);
|
|
||||||
|
|
||||||
switch (st) {
|
|
||||||
case G_IO_STATUS_AGAIN :
|
|
||||||
g_message("again");
|
|
||||||
break;
|
|
||||||
case G_IO_STATUS_NORMAL:
|
|
||||||
g_message("normal [%s]", _buf);
|
|
||||||
// faire un truc
|
|
||||||
g_free(_buf);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
g_message("something else");
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#if 0
|
|
||||||
static gchar buf[BUF_SIZE];
|
|
||||||
gsize len;
|
|
||||||
|
|
||||||
g_io_channel_read_chars(source, buf, BUF_SIZE, &len, NULL);
|
|
||||||
dump_buf(len, buf);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
return TRUE;
|
|
||||||
}
|
|
||||||
|
|
||||||
gboolean timeout_callback(gpointer data) {
|
|
||||||
|
|
||||||
const char* msg = "GT\n";
|
|
||||||
gsize bw;
|
|
||||||
|
|
||||||
g_io_channel_write_chars(ioc, msg, 3, &bw, NULL);
|
|
||||||
g_io_channel_flush(ioc, NULL);
|
|
||||||
|
|
||||||
return TRUE;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
int main ( int argc, char** argv) {
|
|
||||||
|
|
||||||
sp = serial_port_new();
|
|
||||||
serial_port_open(sp, "/dev/ttyUSB1", configure_term);
|
|
||||||
|
|
||||||
ioc = g_io_channel_unix_new(sp->fd);
|
|
||||||
g_io_channel_set_encoding(ioc, NULL, NULL);
|
|
||||||
g_io_channel_set_flags (ioc,G_IO_FLAG_NONBLOCK, NULL );
|
|
||||||
g_io_add_watch (ioc, G_IO_IN, on_serial_data_received, NULL);
|
|
||||||
|
|
||||||
|
|
||||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
|
||||||
|
|
||||||
IvyInit ("Wind_Tunnel", "Wind_Tunnel READY", NULL, NULL, NULL, NULL);
|
|
||||||
IvyStart("127.255.255.255");
|
|
||||||
|
|
||||||
g_timeout_add(500, timeout_callback, NULL);
|
|
||||||
|
|
||||||
g_main_loop_run(ml);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
@@ -1,89 +0,0 @@
|
|||||||
/*
|
|
||||||
pc2rc serial port functions
|
|
||||||
Copyright (C) 2001 Antoine Drouin
|
|
||||||
|
|
||||||
This file is part of paparazzi.
|
|
||||||
|
|
||||||
paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
it under the terms of the GNU General Public License as published by
|
|
||||||
the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
any later version.
|
|
||||||
|
|
||||||
paparazzi is distributed in the hope that it will be useful,
|
|
||||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
GNU General Public License for more details.
|
|
||||||
|
|
||||||
You should have received a copy of the GNU General Public License
|
|
||||||
along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
Boston, MA 02111-1307, USA.
|
|
||||||
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include "serial_port.h"
|
|
||||||
|
|
||||||
#define TRACE(type,fmt,args...)
|
|
||||||
#define TRACE_ERROR 1
|
|
||||||
|
|
||||||
struct SerialPort* serial_port_new() {
|
|
||||||
struct SerialPort* this = malloc(sizeof(struct SerialPort));
|
|
||||||
return this;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* opens serial port and setup the terminal
|
|
||||||
*/
|
|
||||||
guint
|
|
||||||
serial_port_open(struct SerialPort* this, const char* device,
|
|
||||||
void(*term_conf_callback)(struct termios*, speed_t*)) {
|
|
||||||
speed_t speed;
|
|
||||||
if ((this->fd = open(device, O_RDWR)) < 0) {
|
|
||||||
TRACE(TRACE_ERROR,"opening %s (%s)\n", device, strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
if (tcgetattr(this->fd, &this->orig_termios) < 0) {
|
|
||||||
TRACE(TRACE_ERROR,"getting term settings (%s)\n", strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
this->cur_termios = this->orig_termios;
|
|
||||||
term_conf_callback(&this->cur_termios, &speed);
|
|
||||||
if (cfsetispeed(&this->cur_termios, speed)) {
|
|
||||||
TRACE(TRACE_ERROR,"setting term speed (%s)\n", strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
if (tcsetattr(this->fd, TCSADRAIN, &this->cur_termios)) {
|
|
||||||
TRACE(TRACE_ERROR,"setting term attributes (%s)\n", strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* closes serial port and restore term settings
|
|
||||||
*/
|
|
||||||
guint
|
|
||||||
serial_port_close(struct SerialPort* this) {
|
|
||||||
if (tcflush(this->fd, TCIOFLUSH)) {
|
|
||||||
TRACE(TRACE_ERROR,"flushing (%s)\n", strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
if (tcsetattr(this->fd, TCSADRAIN, &this->orig_termios)) { // Restore modes.
|
|
||||||
TRACE(TRACE_ERROR,"restoring term attributes (%s)\n", strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
if (close(this->fd)) {
|
|
||||||
TRACE(TRACE_ERROR,"closing (%s)\n", strerror(errno));
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,20 +0,0 @@
|
|||||||
#ifndef SERIAL_PORT_H
|
|
||||||
#define SERIAL_PORT_H
|
|
||||||
|
|
||||||
#include <termios.h>
|
|
||||||
#include <glib.h>
|
|
||||||
|
|
||||||
struct SerialPort {
|
|
||||||
int fd; /* serial device fd */
|
|
||||||
struct termios orig_termios; /* saved tty state structure */
|
|
||||||
struct termios cur_termios; /* tty state structure */
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
struct SerialPort* serial_port_new();
|
|
||||||
guint serial_port_open(struct SerialPort* this, const char* device,
|
|
||||||
void(*term_conf_callback)(struct termios*, speed_t*));
|
|
||||||
guint serial_port_close(struct SerialPort* this);
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
|
||||||
Reference in New Issue
Block a user