mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +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