diff --git a/controls/model/logAnalysis.m b/controls/model/logAnalysis.m
index 06511cabce98b8521383109b8eaa3852429f6f23..8875fdb5cd7795e672a071bfd660023edcfd0815 100644
--- a/controls/model/logAnalysis.m
+++ b/controls/model/logAnalysis.m
@@ -11,12 +11,14 @@ time_model_40ms = pitch_setpoint_model.time(indices_40ms);
time_model_5ms = x_command_model.time(indices_5ms);
% Pull x control structure data
+x_vel_setpoint_model_data = x_vel_setpoint_model.signals.values(indices_40ms);
pitch_setpoint_model_data = pitch_setpoint_model.signals.values(indices_40ms);
pitchrate_setpoint_model_data = pitchrate_setpoint_model.signals.values(indices_5ms);
x_command_model_data = x_command_model.signals.values(indices_5ms);
x_position_model_data = x_position_model.signals.values(indices_40ms);
% Pull y control structure data
+y_vel_setpoint_model_data = y_vel_setpoint_model.signals.values(indices_40ms);
roll_setpoint_model_data = roll_setpoint_model.signals.values(indices_40ms);
rollrate_setpoint_model_data = rollrate_setpoint_model.signals.values(indices_5ms);
y_command_model_data = y_command_model.signals.values(indices_5ms);
@@ -38,10 +40,8 @@ PWM2_model = motorCommands.signals.values(indices_5ms, 3);
PWM3_model = motorCommands.signals.values(indices_5ms, 4);
% Pull accelerometer readings from model
-pitch_accel = angle_IMU_reading.signals.values(2, 1, indices_5ms);
-pitch_accel = reshape(pitch_accel, [length(pitch_accel) , 1] );
-roll_accel = angle_IMU_reading.signals.values(1, 1, indices_5ms);
-roll_accel = reshape(roll_accel, [length(roll_accel) , 1] );
+pitch_accel = angle_IMU_reading.signals.values(indices_5ms, 2);
+roll_accel = angle_IMU_reading.signals.values(indices_5ms, 1);
% Pull mahony filter data
pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
@@ -49,17 +49,26 @@ roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
%% Plot x control structure
-% Plot lateral controller output
+% Plot x position controller output
figure(1); ax1 = subplot(2, 2, 1);
+stairs(time, x_vel_raw, '.-'); hold on; grid minor;
+stairs(time_model_40ms, x_vel_setpoint_model_data, '.-'); hold off;
+title('X Position Controller Output');
+xlabel('Time (s)');
+ylabel('Velocity (m/s)');
+legend('Log', 'Model', 'location', 'northwest');
+
+% Plot lateral controller output
+ax2 = subplot(2, 2, 2);
stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
-title('Lateral Controller Output');
+title('X Velocity Controller Output');
xlabel('Time (s)');
ylabel('\theta (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot pitch controller output
-ax2 = subplot(2, 2, 2);
+ax3 = subplot(2, 2, 3);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
title('Pitch Controller Output');
@@ -68,7 +77,7 @@ ylabel('d\theta/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot x controller command
-ax3 = subplot(2, 2, 3);
+ax4 = subplot(2, 2, 4);
stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
title('Pitch Rate Controller Output');
@@ -76,30 +85,31 @@ xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
-% Plot x position
-ax4 = subplot(2, 2, 4);
-stairs(time, x_position, '.-'); hold on; grid minor;
-stairs(time_model_40ms, x_position_model_data, '.-'); hold off;
-title('X Position');
-xlabel('Time (s)');
-ylabel('Position (m)');
-legend('Log', 'Model', 'location', 'northwest');
-
linkaxes([ax1, ax2, ax3, ax4], 'x');
%% Plot y control structure
-% Plot longitude controller output
+% Plot y position controller output
figure(2); ax1 = subplot(2, 2, 1);
+stairs(time, y_vel_raw, '.-'); hold on; grid minor;
+stairs(time_model_40ms, y_vel_setpoint_model_data, '.-'); hold off;
+title('Y Position Controller Output');
+xlabel('Time (s)');
+ylabel('Velocity (m/s)');
+legend('Log', 'Model', 'location', 'northwest');
+
+% Plot y velocity controller output
+ax2 = subplot(2, 2, 2);
stairs(time, roll_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off;
-title('Longitude Controller Output ');
+title('Y Velocity Controller Output ');
xlabel('Time (s)');
ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest');
+
% Plot roll controller output
-ax2 = subplot(2, 2, 2);
+ax3 = subplot(2, 2, 3);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
title('Roll Controller Output');
@@ -108,22 +118,13 @@ ylabel('d\phi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot y controller command
-ax3 = subplot(2, 2, 3);
+ax4 = subplot(2, 2, 4);
stairs(time, y_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
title('Y Command');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
-
-% Plot y position
-ax4 = subplot(2, 2, 4);
-stairs(time, y_position, '.-'); hold on; grid minor;
-stairs(time_model_40ms, y_position_model_data, '.-'); hold off;
-title('Y Position');
-xlabel('Time (s)');
-ylabel('Position (m)');
-legend('Log', 'Model', 'location', 'northwest');
linkaxes([ax1, ax2, ax3, ax4], 'x');
@@ -140,7 +141,7 @@ legend('Log', 'Model', 'location', 'northwest');
% Plot z position
ax2 = subplot(2, 1, 2);
-stairs(time, z_position, '.-'); hold on; grid minor;
+stairs(time, z_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
title('Z Position');
xlabel('Time (s)');
@@ -171,7 +172,7 @@ legend('Log', 'Model', 'location', 'northwest');
% Plot yaw position
ax3 = subplot(2, 2, 3);
-stairs(time, yaw_value, '.-'); hold on; grid minor;
+stairs(time, yaw_angle_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
title('Yaw Position');
xlabel('Time (s)');
@@ -218,31 +219,33 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
%% Plot output of complimentary filter
figure(8); ax1 = subplot(2, 1, 1);
-stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
-stairs(time, (pitch_measured_IMU + 0.02) * (180/pi), '.-');
-%stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
-stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
+%stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
+stairs(time, (pitch_measured_IMU) * (180/pi), '.-'); hold on; grid minor;
+stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
+%stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (degrees)');
-legend('VRPN','IMU', 'Model', 'location', 'northwest');
+%legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
+legend('IMU', 'Model','location', 'northwest');
ax2 = subplot(2, 1, 2);
-stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
-stairs(time, roll_measured_IMU * (180/pi), '.-');
-%stairs(time_model_5ms, roll_accel * (180/pi), '.-');
-stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off;
+%stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
+stairs(time, roll_measured_IMU * (180/pi), '.-'); hold on; grid minor;
+stairs(time_model_5ms, roll_accel * (180/pi), '.-');
+%stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off;
title('Roll Complementary Filter Output');
xlabel('Time (s)');
ylabel('Roll Angle (degrees)');
-legend('VRPN','IMU', 'Model', 'location', 'northwest');
+%legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
+legend('IMU', 'Model','location', 'northwest');
linkaxes([ax1, ax2], 'x');
%% Plot VRPN Position
figure(9); ax1 = subplot(3, 1, 1);
-stairs(time, x_position, '.-'); hold on; grid minor;
+stairs(time, x_pos_raw, '.-'); hold on; grid minor;
%stairs(time_model_40ms, x_position_model_data, '.-');
stairs(time, x_setpoint, '.-');
title('X position');
@@ -251,18 +254,17 @@ ylabel('X position');
legend('X position', 'X position model', 'X setpoint');
ax2 = subplot(3, 1, 2);
-stairs(time, y_position, '.-'); hold on; grid minor;
+stairs(time, y_pos_raw, '.-'); hold on; grid minor;
%stairs(time_model_40ms, y_position_model_data, '.-');
-stairs(time, x_setpoint, '.-');
-
+stairs(time, y_setpoint, '.-');
title('Y position');
xlabel('Time (s)');
ylabel('Y position');
legend('Y position', 'Y position model', 'Y setpoint');
ax3 = subplot(3, 1, 3);
-stairs(time, y_position, '.-'); hold on; grid minor;
-stairs(time_model_40ms, y_position_model_data, '.-');
+stairs(time, z_pos_raw, '.-'); hold on; grid minor;
+stairs(time_model_40ms, z_position_model_data, '.-');
title('Z position');
xlabel('Time (s)');
ylabel('Z position');
@@ -273,18 +275,21 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot Gyro Data
figure(10); ax1 = subplot(3, 1, 1);
stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor;
+stairs(time_model_5ms, gyro_data_x, '.-');
title('gyro x');
xlabel('Time (s)');
ylabel('p (rad/s)');
ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
+stairs(time_model_5ms, gyro_data_y, '.-');
title('gyro y');
xlabel('Time (s)');
ylabel('q (rad/s)');
ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
+stairs(time_model_5ms, gyro_data_z, '.-');
title('gyro z');
xlabel('Time (s)');
ylabel('r (rad/s)');
@@ -294,18 +299,21 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot Accel Data
figure(11); ax1 = subplot(3, 1, 1);
stairs(time, raw_accel_data_x, '.-'); hold on; grid minor;
+stairs(time_model_5ms, accel_data_x, '.-');
title('accel x');
xlabel('Time (s)');
ylabel('accel x (g)');
ax2 = subplot(3, 1, 2);
-stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
+stairs(time, raw_accel_data_y, '.-'); hold on; grid minor;
+stairs(time_model_5ms, accel_data_y, '.-');
title('accel y');
xlabel('Time (s)');
ylabel('accel y (g)');
ax3 = subplot(3, 1, 3);
-stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
+stairs(time, raw_accel_data_z, '.-'); hold on; grid minor;
+stairs(time_model_5ms, accel_data_z, '.-');
title('accel z');
xlabel('Time (s)');
ylabel('accel (z)');
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 3ad190cf93f61fe0b5149b9e8f70b9ab297e75a4..0b7971269fcd3f3bef9de1f2b0e2f69a8a83d203 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -6,7 +6,7 @@
runtime = 20;
% Model Parameters
- m = 1.140; % Quadrotor + battery mass
+ m = 1.216; % Quadrotor + battery mass
g = 9.81; % Acceleration of gravity
Jxx = 0.0130; % Quadrotor and battery motor of inertia around bx (pitch)
Jyy = 0.0140; % Quadrotor and battery motor of inertia around by (roll)
@@ -19,12 +19,12 @@
rhy = 0.16; % Y-axis distance from center of mass to a rotor hub
rhz = 0.03; % Z-axis distance from center of mass to a rotor hub
r_oc = [0; 0; 0]; % Vector from origin to center of mass
- Rm = 0.2308; % Motor resistance
+ Rm = 0.235; % Motor resistance
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
If = 0.3836; % Motor internal friction current
- Pmin = 1e5; % Minimum zybo output duty cycle command
- Pmax = 2e5; % Maximum zybo output duty cycle command
+ Pmin = 0; % Minimum zybo output duty cycle command
+ Pmax = 1; % Maximum zybo output duty cycle command
Tc = 0.04; % Camera system sampling period
Tq = 0.005; % Quad sampling period
tau_c = 0; % Camera system total latency
@@ -32,9 +32,18 @@
x_controlled_o = 0; % Equilibrium lateral controller output
y_controlled_o = 0; % Equilibrium longitudinal controller output
yaw_controlled_o = 0; % Equilibrium yaw controller output
- Kp_mahony = 0.2; % Proportional term for mahony filter
+ Kp_mahony = 0.4; % Proportional term for mahony filter
Ki_mahony = 0.001; % Integral term for mahony filter
+ % Define Biquad Filter Parameters
+ b0 = 0.020083;
+ b1 = 0.040167;
+ b2 = 0.020083;
+ a0 = 1;
+ a1 = -1.561;
+ a2 = 0.6414;
+ SOS_Matrix = [b0, b1, b2, a0, a1, a2];
+
% Determine Initial Conditions
% Equilibrium rotor speeds
@@ -94,56 +103,74 @@ if logAnalysisToggle == 1
time = dataStruct.Time.data;
time = time - time(1);
time_indices = length(time);
-
-% time = 0:0.005001:max(time);
-% time = time(1:time_indices);
-
+
runtime = max(time);
% Determine x position error
x_setpoint = dataStruct.X_Setpoint_Constant.data;
- x_position = dataStruct.VRPN_X_Constant.data;
- x_error = timeseries(x_setpoint - x_position, time);
+ x_pos_raw = dataStruct.VRPN_X_Constant.data;
+ x_error = timeseries(x_setpoint - x_pos_raw, time);
+ x_position = timeseries(x_pos_raw, time);
% Determine y position error
y_setpoint = dataStruct.Y_Setpoint_Constant.data;
- y_position = dataStruct.VRPN_Y_Constant.data;
- y_error = timeseries(y_setpoint - y_position, time);
+ y_pos_raw = dataStruct.VRPN_Y_Constant.data;
+ y_error = timeseries(y_setpoint - y_pos_raw, time);
+ y_position = timeseries(y_pos_raw, time);
% Determine z position error
z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
- z_position = dataStruct.VRPN_Alt_Constant.data;
- z_error = timeseries(z_setpoint - z_position, time);
+ z_pos_raw = dataStruct.VRPN_Alt_Constant.data;
+ z_error = timeseries(z_setpoint - z_pos_raw, time);
+ z_position = timeseries(z_pos_raw, time);
+
+ % Determine x velocity error
+ x_vel_setpoint = dataStruct.X_pos_PID_Correction.data;
+ x_vel_raw = dataStruct.X_Vel_Correction.data;
+ x_vel_error = timeseries(x_vel_setpoint - x_vel_raw, time);
+ x_vel = timeseries(x_vel_raw, time);
+
+ % Determine y velocity error
+ y_vel_setpoint = dataStruct.Y_pos_PID_Correction.data;
+ y_vel_raw = dataStruct.Y_Vel_Correction.data;
+ y_vel_error = timeseries(y_vel_setpoint - y_vel_raw, time);
+ y_vel = timeseries(y_vel_raw, time);
% Determine pitch error
pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
- pitch_value = dataStruct.Pitch_trim_add_Sum.data;
- pitch_error = timeseries(pitch_setpoint - pitch_value, time);
+ pitch_angle_raw = dataStruct.Pitch_trim_add_Sum.data;
+ pitch_error = timeseries(pitch_setpoint - pitch_angle_raw, time);
+ pitch_angle = timeseries(pitch_angle_raw, time);
% Determine roll error
roll_setpoint = dataStruct.Y_pos_PID_Correction.data;
- roll_value = dataStruct.Roll_Constant.data;
- roll_error = timeseries(roll_setpoint - roll_value, time);
-
+ roll_angle_raw = dataStruct.Roll_Constant.data;
+ roll_error = timeseries(roll_setpoint - roll_angle_raw, time);
+ roll_angle = timeseries(roll_angle_raw, time);
+
% Determine yaw error
yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
- yaw_value = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
- yaw_error = timeseries(yaw_setpoint - yaw_value, time);
+ yaw_angle_raw = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
+ yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
+ yaw_angle = timeseries(yaw_angle_raw, time);
% Determine pitch rate error
pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data;
- pitchrate_value = dataStruct.gyro_y.data;
- pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_value, time);
+ pitchrate_raw = dataStruct.gyro_y.data;
+ pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_raw, time);
+ pitchrate = timeseries(pitchrate_raw, time);
% Determine roll rate error
rollrate_setpoint = dataStruct.Roll_PID_Correction.data;
- rollrate_value = dataStruct.gyro_x.data;
- rollrate_error = timeseries(rollrate_setpoint - rollrate_value, time);
+ rollrate_raw = dataStruct.gyro_x.data;
+ rollrate_error = timeseries(rollrate_setpoint - rollrate_raw, time);
+ rollrate = timeseries(rollrate_raw, time);
% Determine yaw rate error
yawrate_setpoint = dataStruct.Yaw_PID_Correction.data;
- yawrate_value = dataStruct.gyro_z.data;
- yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time);
+ yawrate_raw = dataStruct.gyro_z.data;
+ yawrate_error = timeseries(yawrate_setpoint - yawrate_raw, time);
+ yawrate = timeseries(yawrate_raw, time);
% Pull motor commands from log
x_command = dataStruct.Pitch_Rate_PID_Correction.data;
@@ -152,10 +179,10 @@ if logAnalysisToggle == 1
yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
% Determine signal mix PWM values
- PWM0 = dataStruct.Signal_Mixer_PWM_0.data;
- PWM1 = dataStruct.Signal_Mixer_PWM_1.data;
- PWM2 = dataStruct.Signal_Mixer_PWM_2.data;
- PWM3 = dataStruct.Signal_Mixer_PWM_3.data;
+ PWM0 = dataStruct.Signal_Mixer_MOTOR_0.data;
+ PWM1 = dataStruct.Signal_Mixer_MOTOR_1.data;
+ PWM2 = dataStruct.Signal_Mixer_MOTOR_2.data;
+ PWM3 = dataStruct.Signal_Mixer_MOTOR_3.data;
PWM_arr = ...
[PWM0, PWM1, PWM2, PWM3];
motor_commands = timeseries(PWM_arr, time);
@@ -191,11 +218,11 @@ if logAnalysisToggle == 1
pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data;
roll_measured_VRPN = dataStruct.VRPN_Roll_Constant.data;
-% % Set height_controlled_o to initial throttle command
+% Set height_controlled_o to initial throttle command
% height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
%
-% % Set rotor speed initial conditions to there calculated values based on
-% % initial throttle control
+% Set rotor speed initial conditions to there calculated values based on
+% initial throttle control
% omega0_o = (sqrt(((height_controlled_o ...
% - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ...
% Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm * ...
@@ -204,22 +231,22 @@ if logAnalysisToggle == 1
% omega2_o = omega0_o;
% omega3_o = omega0_o;
%
-% % Set initial positions
+% Set initial positions
% x_o = x_position(1);
% y_o = y_position(1);
% z_o = z_position(1);
%
-% % Set initial velocities
+% Set initial velocities
% x_vel_o = (x_position(1) - x_position(2))/(time(1) - time(2));
% y_vel_o = (y_position(1) - y_position(2))/(time(1) - time(2));
% z_vel_o = (z_position(1) - z_position(2))/(time(1) - time(2));
%
-% % Equilibrium angles
+% Equilibrium angles
% roll_o = roll_measured_VRPN(1);
% pitch_o = pitch_measured_VRPN(1);
% yaw_o = 0;
%
-% % Equilibrium angular rates
+% Equilibrium angular rates
% rollrate_o = (roll_measured_VRPN(1) - roll_measured_VRPN(2))/(time(1) - time(2));
% pitchrate_o = (pitch_measured_VRPN(1) - pitch_measured_VRPN(2))/(time(1) - time(2));
% yawrate_o = 0;
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 62dc0a21547c1eff7b1e558e74d80030e2955325..d5bc5cf9bf30f0bb5a79b4167ac79bdb8b1be8a8 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ
diff --git a/quad/Makefile b/quad/Makefile
index dde14bff55200e891e367b6a4e126c5edb97611e..b6838e4bfdc801ae4c8401f8c06eb5941e706557 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -38,7 +38,7 @@ test: all
$(MAKE) -C src/queue test
$(MAKE) -C src/computation_graph test
$(MAKE) -C src/quad_app test
- # ruby scripts/tests/test_safety_checks.rb
+ ruby scripts/tests/test_safety_checks.rb
# ruby scripts/tests/test_unix_uart.rb
# ruby scripts/tests/run_virtual_test_flight.rb
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index 9e7d693e6ee8459bedf24cc503a6265dcd8e6db0..3e45b244a059d23de7461a1ea00d89d30acac0bd 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -16,15 +16,12 @@ Timeout::timeout(30) {
puts("Setting up...")
-
- sleep 1
-
# Start virtual quad
quad_pid = Process.spawn("./virt-quad -q",
{ :rlimit_as => 536870912, # 512 MiB total RAM
:rlimit_stack => 1048576}) # 1 MiB stack
- delay_spin_cursor(5)
+ delay_spin_cursor(1)
#################
# Begin Tests
@@ -35,8 +32,11 @@ Timeout::timeout(30) {
puts("Beginning tests...")
# Set gravity and gear
- File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
- File.write(GEAR, GEAR_OFF)
+ set_gear GEAR_OFF
+ set_flap FLAP_ON
+ `./virt-quad set rc_roll 0.498`
+ `./virt-quad set rc_pitch 0.497`
+ `./virt-quad set rc_yaw 0.498`
puts("Check that motors are off at startup")
check_motors_are_off "Motors weren't off at startup! How dangerous!"
@@ -47,7 +47,7 @@ Timeout::timeout(30) {
puts("Check that increasing the throttle does nothing to motors")
# (because gear is still off)
for i in (THROTTLE_MIN..THROTTLE_MAX).step(0.01)
- File.write(THROTTLE, i)
+ set_throttle(i)
check_motors_are_off "Was able to turn on motors with GEAR off! Yikes!"
check_led(0, "LED turned on while GEAR was off!")
sleep 0.005
@@ -55,47 +55,44 @@ Timeout::timeout(30) {
puts("Check that flipping gear to 1 while throttle is high does nothing")
# (motors should still be off, LED should still be off)
- File.write(GEAR, GEAR_ON)
- sleep 0.5
+ sleep 0.01
check_motors_are_off "Motors turned on by gear while rc throttle was high! How dangerous!"
i = THROTTLE_MAX
while i > THROTTLE_MID
i -= 0.01
- File.write(THROTTLE, i)
+ set_throttle(i)
check_motors_are_off "Motors turned on while GEAR was off. Yikes!"
check_led(0, "LED turned on while GEAR was off!")
sleep 0.005
end
# (swtich GEAR back to off and bring throttle off)
- File.write(GEAR, GEAR_OFF)
- File.write(THROTTLE, THROTTLE_MIN)
+ set_gear GEAR_OFF
+ set_throttle THROTTLE_MIN
puts("Check that the LED turns on when gear is flipped on")
# (motors should still be off because our throttle is low)
- File.write(GEAR, GEAR_ON)
- sleep 0.5
+ set_gear GEAR_ON
+ sleep 0.01
check_motors_are_off "Motors turned on while throttle was low! Yikes!"
check_led(1, "LED didn't turn on when GEAR was switched on!")
puts("Check that motors turn on")
- File.write(THROTTLE, THROTTLE_MID)
- averages = get_motor_averages
- average = (averages[0] + averages[1] + averages[2] + averages[3])/4
- puts averages, "(#{average})"
- assert average.between?(THROTTLE_EIGHTH, MOTOR_MAX)
+ set_throttle THROTTLE_MID
+ spinner = Thread.new { delay_spin_cursor(5) }
+ motors = get_motor_averages(100, 5)
+ spinner.exit
+ p motors
+ motors.each { |value| assert_operator(value, :>, THROTTLE_EIGHTH) }
puts("Check that gear switch kills the motors")
- # (and that light goes off)
- File.write(GEAR, GEAR_OFF)
- sleep 0.5
+ set_gear GEAR_OFF
+ sleep 0.01
check_motors_are_off "Motors didn't turn off when GEAR was switched off! How dangerous!"
- #check_led 0
# (Bring the RC throttle back down)
- File.write(THROTTLE, THROTTLE_MIN)
+ set_throttle THROTTLE_MIN
- sleep 1
puts "All safety checks passed."
ensure
diff --git a/quad/scripts/tests/testing_library.rb b/quad/scripts/tests/testing_library.rb
index 5324eacdd4dd8f9ad1d73aa79263a37f3b1a30e8..9da10aef8f1d80cad84baf1ba22f1c0054c42154 100644
--- a/quad/scripts/tests/testing_library.rb
+++ b/quad/scripts/tests/testing_library.rb
@@ -9,6 +9,8 @@ MOTOR_MIN = 0.0
MOTOR_MAX = 1.0
GEAR_ON = 0.70800
GEAR_OFF = 0.18300
+FLAP_ON = 0.9
+FLAP_OFF = 0.1
GRAVITY = 4096
MOTOR1 = "virt-quad-fifos/motor1"
@@ -33,47 +35,41 @@ require 'timeout'
require 'thread'
include Test::Unit::Assertions
-$fifos = Hash.new
+# Utility functions
+def check_motors_are_off(msg)
+ motors = `./virt-quad get motors`.chomp.split("\n").map { |s| s.to_f }
+ motors.each { |val|
+ assert_operator(val, :<=, THROTTLE_MIN, msg)
+ }
+end
-def read_fifo_num(f)
- if not $fifos.key?(f)
- $fifos[f] = File.open(f)
- end
- $fifos[f].read().chomp.split("\n").last.to_i
+def check_led(is_on, msg)
+ led = `./virt-quad get led`.to_i
+ assert_equal(is_on, led, msg)
end
-# Utility functions
-def check_motors_are_off(msg)
- motor1 = read_fifo_num(MOTOR1)
- motor2 = read_fifo_num(MOTOR2)
- motor3 = read_fifo_num(MOTOR3)
- motor4 = read_fifo_num(MOTOR4)
- assert_operator(motor1, :<=, THROTTLE_MIN, msg)
- assert_operator(motor2, :<=, THROTTLE_MIN, msg)
- assert_operator(motor3, :<=, THROTTLE_MIN, msg)
- assert_operator(motor4, :<=, THROTTLE_MIN, msg)
+def set_gear(gear)
+ `./virt-quad set rc_gear #{gear}`
end
-def get_motor_averages
- motors = [[], [], [], []]
- for i in 0..100
- motors[0].push(read_fifo_num(MOTOR1))
- motors[1].push(read_fifo_num(MOTOR2))
- motors[2].push(read_fifo_num(MOTOR3))
- motors[3].push(read_fifo_num(MOTOR4))
- sleep 0.010
- end
- average = []
- average[0] = motors[0].inject(:+).to_f / motors[0].size
- average[1] = motors[1].inject(:+).to_f / motors[1].size
- average[2] = motors[2].inject(:+).to_f / motors[2].size
- average[3] = motors[3].inject(:+).to_f / motors[3].size
- average
+def set_flap(flap)
+ `./virt-quad set rc_flap #{flap}`
end
-def check_led(is_on, msg)
- led = read_fifo_num(LED)
- assert_equal(is_on, led, msg)
+def set_throttle(throttle)
+ `./virt-quad set rc_throttle #{throttle}`
+end
+
+def get_motor_averages(times, total_time)
+ motor_sums = [0.0, 0.0, 0.0, 0.0]
+ for i in 0..times
+ motors = `./virt-quad get motors`.chomp.split("\n").map { |s| s.to_f }
+ for i in 0..3
+ motor_sums[i] += motors[i]
+ end
+ sleep (total_time.to_f / times.to_f)
+ end
+ motor_sums.map {|val| val / times}
end
def delay_spin_cursor(delay)
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 5cd49342dcfffdaeb24649d18ca1555219200438..6e85fa4a15c835be8e72ae7d47b2c39510f07cc6 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -15,26 +15,27 @@
extern int Xil_AssertWait;
+int start_condition_is_met(float *rc_commands) {
+ return rc_commands[THROTTLE] < 0.125 &&
+ gear_is_engaged(rc_commands[GEAR]) &&
+ flap_is_engaged(rc_commands[FLAP]);
+}
+
int protection_loops(modular_structs_t *structs)
{
- float rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
-
- struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
- read_rec_all(rc_receiver, rc_commands);
-
- while(rc_commands[THROTTLE] < 0.075 || // wait for RC receiver to connect to transmitter
- rc_commands[THROTTLE] > 0.125 || // Wait until throttle is low
- read_kill(rc_commands[GEAR]) || // Wait until gear switch is engaged (so you don't immediately break out of the main loop below)
- !read_flap(rc_commands[FLAP])) // Wait for the flight mode to be set to manual
- {
- process_received(structs);
- read_rec_all(rc_receiver, rc_commands);
- }
-
- // let the pilot/observers know that the quad is now active
- MIO7_led_on();
-
- return 0;
+ float rc_commands[6];
+ struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
+ read_rec_all(rc_receiver, rc_commands);
+
+ while(!start_condition_is_met(rc_commands)) {
+ process_received(structs);
+ read_rec_all(rc_receiver, rc_commands);
+ }
+
+ // let the pilot/observers know that the quad is now active
+ MIO7_led_on();
+
+ return 0;
}
int init_structs(modular_structs_t *structs) {
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index e982432eafb4b624990b3da5b95846cc672abc50..698d453c7734dc2fd4dd5975d722fcdfa2ac6581 100644
--- a/quad/src/quad_app/util.c
+++ b/quad/src/quad_app/util.c
@@ -49,15 +49,23 @@ int hexStrToInt(char *buf, int startIdx, int endIdx) {
* Otherwise, do nothing
*/
int read_kill(float gear) {
- if(gear < GEAR_MID)
- return 1;
- return 0;
+ if (gear_is_engaged(gear)) return 0;
+ else return 1;
+}
+
+int gear_is_engaged(float gear) {
+ if (gear > GEAR_MID) return 1;
+ else return 0;
}
int read_flap(float flap) {
- if(flap > FLAP_MID)
- return 1;
- return 0;
+ if (flap_is_engaged(flap)) return 1;
+ else return 0;
+}
+
+int flap_is_engaged(float flap) {
+ if (flap > FLAP_MID) return 1;
+ else return 0;
}
/**
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index 390bafd9d5121af93ebef8e0a16c1340bea1fead..d675428293bc3108673bf953f78e089f8877caea 100644
--- a/quad/src/quad_app/util.h
+++ b/quad/src/quad_app/util.h
@@ -14,6 +14,8 @@
void read_rec_all(struct RCReceiverDriver *rc_receiver, float *mixer);
int read_kill(float gear);
int read_flap(float flap);
+int gear_is_engaged(float gear);
+int flap_is_engaged(float flap);
void kill_motors(struct MotorDriver *motors);
int build_int(u8 *buff);
diff --git a/quad/src/virt_quad/Makefile b/quad/src/virt_quad/Makefile
index acf6d27cbbea07d639fadfda691a01d6edd89fb7..241f5e48b0fcca40287660a00052843bba10fa0c 100644
--- a/quad/src/virt_quad/Makefile
+++ b/quad/src/virt_quad/Makefile
@@ -1,7 +1,7 @@
TOP=../..
NAME = virt-quad
-REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm -lpthread
+REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm -lpthread -lrt
include $(TOP)/executable.mk
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index 85e223f2896a317fd5f1c1fa79f6fc60cb7a8772..296d30fbf58be8bd9c9db9a5cb7d97f5dea8e395 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -9,9 +9,26 @@
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include "controllers.h"
+#include <pthread.h>
+#include <errno.h>
#define VIRT_QUAD_FIFOS_DIR "virt-quad-fifos"
+struct VirtQuadIO {
+ pthread_mutex_t led_lock;
+ int led;
+ pthread_mutex_t motors_lock;
+ float motors[4];
+ pthread_mutex_t rc_lock;
+ float rc_receiver[6];
+};
+
+#define VIRT_QUAD_SHARED_MEMORY "/virt-quad-io"
+
int unix_uart_reset(struct UARTDriver *self);
int unix_uart_write(struct UARTDriver *self, unsigned char c);
int unix_uart_read(struct UARTDriver *self, unsigned char *c);
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index 97228f0bada9ca366f30c145f66c54edae0cedde..aa2c1d5cee52eefc7e6c132534f1f6e5f14c754e 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -7,49 +7,7 @@
#define NUM_INPUTS 7
-void * update_i2c_input_cache(void *);
-
-union val {
- unsigned char b[2];
- unsigned short s;
-};
-
-static char *input_names[NUM_INPUTS];
-static int fifos[NUM_INPUTS];
-static union val cache[NUM_INPUTS];
-
-static short last_dev;
-static short last_reg;
-static short last_val;
-
-static int nums[] = {0, 1, 2, 3, 4, 5};
-static pthread_t workers[NUM_INPUTS];
-
int unix_i2c_reset(struct I2CDriver *self) {
- input_names[0] = "i2c-mpu-accel-x";
- input_names[1] = "i2c-mpu-accel-y";
- input_names[2] = "i2c-mpu-accel-z";
- input_names[3] = "i2c-mpu-gryo-x";
- input_names[4] = "i2c-mpu-gryo-y";
- input_names[5] = "i2c-mpu-gyro-z";
- input_names[6] = "i2c-lidar";
-
- mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
-
- // Start up worker thread whose job is to update the caches
- int i;
- for (i = 0; i < NUM_INPUTS; i += 1) {
- pthread_create(&workers[i], 0, update_i2c_input_cache, &nums[i]);
- }
-
- cache[0].s = 0;
- cache[1].s = 0;
- cache[2].s = 0;
- cache[3].s = 0;
- cache[4].s = 0;
- cache[5].s = 0;
- cache[6].s = 0;
-
return 0;
}
@@ -57,73 +15,12 @@ int unix_i2c_write(struct I2CDriver *self,
unsigned short device_addr,
unsigned char *data,
unsigned int length) {
- if (length == 2) {
- last_dev = device_addr;
- last_reg = data[0];
- last_val = data[1];
- }
- return 0;
+ return -1;
}
int unix_i2c_read(struct I2CDriver *self,
unsigned short device_addr,
unsigned char *buff,
unsigned int length) {
- if (last_dev != device_addr) {
- return -1;
- }
-
- switch (device_addr) {
- case MPU9150_DEVICE_ADDR:
- if (last_reg == ACCEL_GYRO_BASE_ADDR) {
- buff[0] = cache[0].b[0];
- buff[1] = cache[0].b[1];
- buff[2] = cache[1].b[0];
- buff[3] = cache[1].b[1];
- buff[4] = cache[2].b[0];
- buff[5] = cache[2].b[1];
- buff[6] = 0;
- buff[7] = 0;
- buff[8] = cache[3].b[0];
- buff[9] = cache[3].b[1];
- buff[10] = cache[4].b[0];
- buff[11] = cache[4].b[1];
- buff[12] = cache[5].b[0];
- buff[13] = cache[5].b[1];
- }
- else if (last_reg == LIDARLITE_DEVICE_ADDR) {
- buff[0] = cache[6].b[0];
- buff[1] = cache[6].b[0];
- }
- else {
- return -1;
- }
- }
- return 0;
-}
-
-void * update_i2c_input_cache(void *arg) {
- int *cache_index = arg;
- int i = *cache_index;
- char buff[16];
-
- // Setup FIFO
- unlink(input_names[i]);
- char fifoname[64];
- sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
- mkfifo(fifoname, 0666);
- fifos[i] = open(fifoname, O_RDONLY);
-
- // Block while waiting for reads
- while (1) {
- int bytes_read = read(fifos[i], buff, 15);
- if (bytes_read > 0) {
- buff[bytes_read] = '\0';
- unsigned long val = strtoll(buff, NULL, 10);
- cache[i].s = val;
- printf("%s: %ld\n", input_names[i], val);
- }
- pthread_yield();
- }
- return NULL;
+ return -1;
}
diff --git a/quad/src/virt_quad/hw_impl_unix_mio7_led.c b/quad/src/virt_quad/hw_impl_unix_mio7_led.c
index b9b66dd09d0725936a97fc86563fb709693b2416..af831c58025801c30a21e646c643573d83a9ac45 100644
--- a/quad/src/virt_quad/hw_impl_unix_mio7_led.c
+++ b/quad/src/virt_quad/hw_impl_unix_mio7_led.c
@@ -7,25 +7,10 @@
#include <unistd.h>
#include <pthread.h>
-void * output_cached_led();
-
-int on;
-static char *led_fifo_name;
-pthread_t worker;
+extern struct VirtQuadIO *virt_quad_io;
+static int on;
int unix_mio7_led_reset(struct LEDDriver *self) {
- led_fifo_name = VIRT_QUAD_FIFOS_DIR "/mio7-led";
-
- mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
- int i;
- for (i = 0; i < 4; i += 1) {
- unlink(led_fifo_name);
- mkfifo(led_fifo_name, 0666);
- }
-
- // Start up worker thread whose job is to update the caches
- pthread_create(&worker, 0, output_cached_led, NULL);
-
return 0;
}
@@ -33,6 +18,11 @@ int unix_mio7_led_turn_on(struct LEDDriver *self) {
if (!on) {
puts("LED ON");
on = 1;
+
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->led_lock);
+ io->led = on;
+ pthread_mutex_unlock(&io->led_lock);
}
return 0;
}
@@ -41,17 +31,11 @@ int unix_mio7_led_turn_off(struct LEDDriver *self) {
if (on) {
puts("LED OFF");
on = 0;
- }
- return 0;
-}
-void * output_cached_led() {
- char buff[16];
- while (1) {
- int fifo = open(led_fifo_name, O_WRONLY);
- sprintf(buff, "%d\n", on);
- write(fifo, buff, strlen(buff));
- close(fifo);
- pthread_yield();
- }
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->led_lock);
+ io->led = on;
+ pthread_mutex_unlock(&io->led_lock);
+}
+ return 0;
}
diff --git a/quad/src/virt_quad/hw_impl_unix_motor.c b/quad/src/virt_quad/hw_impl_unix_motor.c
index 3ad271b526aff0b45047988280157e1aa5e12b25..98da452a0c8793a02b599d1daebf4176adbd65b1 100644
--- a/quad/src/virt_quad/hw_impl_unix_motor.c
+++ b/quad/src/virt_quad/hw_impl_unix_motor.c
@@ -6,65 +6,18 @@
#include <pthread.h>
#include <unistd.h>
-void * output_cache();
-
-static char *output_pwms[4];
-static float cache[4];
-pthread_t worker;
-static void float_equals(float a, float b);
-
-static int zero = 0;
-static int one = 1;
-static int two = 2;
-static int three = 3;
+extern struct VirtQuadIO *virt_quad_io;
int unix_motor_reset(struct MotorDriver *self) {
- output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/motor1";
- output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/motor2";
- output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/motor3";
- output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/motor4";
-
- mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
-
- // Start up worker thread whose job is to update the caches
- pthread_create(&worker, 0, output_cache, &zero);
- pthread_create(&worker, 0, output_cache, &one);
- pthread_create(&worker, 0, output_cache, &two);
- pthread_create(&worker, 0, output_cache, &three);
-
return 0;
}
int unix_motor_write(struct MotorDriver *self,
unsigned int channel,
float magnitude) {
- if (cache[channel] != magnitude) {
- printf("%s: %.2f\n", output_pwms[channel], magnitude);
- }
- cache[channel] = magnitude;
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->motors_lock);
+ io->motors[channel] = magnitude;
+ pthread_mutex_unlock(&io->motors_lock);
return 0;
}
-
-void * output_cache(void *arg) {
- int *output_index = arg;
- char buff[16];
- int i = *output_index;
-
- // Setup FIFO
- unlink(output_pwms[i]);
- mkfifo(output_pwms[i], 0666);
-
- // Block while waiting for someone to listen
- while (1) {
- int fifo = open(output_pwms[i], O_WRONLY);
- sprintf(buff, "%.2f\n", cache[i]);
- write(fifo, buff, strlen(buff));
- close(fifo);
- pthread_yield();
- }
- return NULL;
-}
-
-static void float_equals(float a, float b) {
- return fabs(a - b) < 0.00001;
-}
diff --git a/quad/src/virt_quad/hw_impl_unix_rc_receiver.c b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
index 229a019cd041d0aaed0636c3805121868eb68835..85cf852c71681b83b6c7748273aa49cf4a659ee9 100644
--- a/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
+++ b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
@@ -5,80 +5,18 @@
#include <fcntl.h>
#include <pthread.h>
-void * update_input_cache();
-
-static char *input_names[6];
-static int fifos[6];
-static float cache[6];
-static pthread_t workers[6];
-static int nums[] = {0, 1, 2, 3, 4, 5};
+extern struct VirtQuadIO *virt_quad_io;
int unix_rc_receiver_reset(struct RCReceiverDriver *self) {
- input_names[0] = "rc-throttle";
- input_names[1] = "rc-roll";
- input_names[2] = "rc-pitch";
- input_names[3] = "rc-yaw";
- input_names[4] = "rc-gear";
- input_names[5] = "rc-flap";
-
- mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
-
- // Start up worker thread whose job is to update the caches
- int i;
- for (i = 0; i < 6; i += 1) {
- pthread_create(&workers[i], 0, update_input_cache, &nums[i]);
- usleep(1000);
- }
-
- cache[0] = THROTTLE_MIN;
- cache[1] = ROLL_CENTER;
- cache[2] = PITCH_CENTER;
- cache[3] = YAW_CENTER;
- cache[4] = GEAR_0;
- cache[5] = FLAP_1;
-
- for (i = 0; i < 6; i += 1) {
- printf("%s: %.2f\n", input_names[i], cache[i]);
- }
-
return 0;
}
int unix_rc_receiver_read(struct RCReceiverDriver *self,
unsigned int channel,
float *magnitude) {
-
- *magnitude = cache[channel];
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->rc_lock);
+ *magnitude = io->rc_receiver[channel];
+ pthread_mutex_unlock(&io->rc_lock);
return 0;
}
-
-void * update_input_cache(void *arg) {
- int *cache_index = arg;
- int i = *cache_index;
- char buff[16];
-
- // Setup FIFO
- unlink(input_names[i]);
- char fifoname[64];
- sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
- mkfifo(fifoname, 0666);
- fifos[i] = open(fifoname, O_RDONLY);
-
- // Block while waiting for reads
- while (1) {
- int bytes_read = read(fifos[i], buff, 15);
- if (bytes_read > 0) {
- buff[bytes_read] = '\0';
- float val = strtof(buff, NULL);
- if (val <= 1 && val >= 0) {
- cache[i] = val;
- printf("%s: %.2f\n", input_names[i], val);
- }
- else {
- printf("%s: Bad value (%f) - input not received\n", input_names[i], val);
- }
- }
- pthread_yield();
- }
- return NULL;
-}
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
index 5f6aaf8dbc9fda7e1adfc1c21fe0ecafa8065e5b..7f9e4447e0c849f9384a78ea7c012436fe384a90 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -4,6 +4,13 @@
#include "quad_app.h"
#include <fcntl.h>
+int handle_io_output(const char *name);
+int handle_io_input(const char *name, const char *value_str);
+void set_shm(float value, float *dest, pthread_mutex_t *lock);
+
+struct VirtQuadIO *virt_quad_io;
+int virt_quad_io_file_descriptor;
+
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_unix_i2c();
hardware->rc_receiver = create_unix_rc_receiver();
@@ -30,7 +37,117 @@ int main(int argc, char *argv[]) {
exit(EXIT_FAILURE);
}
}
+ argc -= optind;
+ argv += optind;
+
+
+ // Decide if we are launching the quad or responding to a request
+ // about a running quad
+ if (argv[0] == NULL) {
+ // Prepare the shared memory for io. Clear memory and initialize.
+ puts("Setting up...");
+ int *fd = &virt_quad_io_file_descriptor;
+ *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR | O_TRUNC, 0666);
+ ftruncate(*fd, sizeof(struct VirtQuadIO));
+ virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
+ pthread_mutexattr_t attr;
+ pthread_mutexattr_init(&attr);
+ pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+ pthread_mutex_init(&virt_quad_io->led_lock, &attr);
+ pthread_mutex_init(&virt_quad_io->motors_lock, &attr);
+ pthread_mutex_init(&virt_quad_io->rc_lock, &attr);
+ puts("Finished setting up.");
+ }
+ else {
+ // Open exising shared memory for io. DO NOT CLEAR.
+ int *fd = &virt_quad_io_file_descriptor;
+ *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR, 0666);
+ virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
+
+ // Meet requests
+ if (strcmp(argv[0], "get") == 0 && argv[1] != NULL) {
+ handle_io_output(argv[1]);
+ }
+ else if (strcmp(argv[0], "set") == 0 && argv[1] != NULL && argv[2] != NULL) {
+ handle_io_input(argv[1], argv[2]);
+ }
+ return 0;
+ }
+ puts("Starting the quad application");
quad_main(setup_hardware);
return 0;
}
+
+/**
+ * The user wants to read an output by name. Get the output
+ * and print to stdout.
+ */
+int handle_io_output(const char *name) {
+ int ret = 0;
+ struct VirtQuadIO *io = virt_quad_io;
+ if (strcmp(name, "led") == 0) {
+ pthread_mutex_lock(&io->led_lock);
+ printf("%d\n", io->led);
+ pthread_mutex_unlock(&io->led_lock);
+ }
+ else if (strcmp(name, "motors") == 0) {
+ pthread_mutex_lock(&io->motors_lock);
+ int i;
+ for (i = 0; i < 4; i += 1) {
+ printf("%f\n", io->motors[i]);
+ }
+ pthread_mutex_unlock(&io->motors_lock);
+ }
+ else {
+ puts("Given output not recognized.");
+ ret = -1;
+ }
+ return ret;
+}
+
+/**
+ * The user wants to set an input by name, and has supplied
+ * a value. Set the appropriate input to the given value.
+ */
+int handle_io_input(const char *name, const char *value_str) {
+ int ret = 0;
+ errno = 0;
+ float value = strtof(value_str, NULL);
+ if (errno) {
+ ret = -1;
+ puts("Given number format was bad.");
+ }
+ else {
+ struct VirtQuadIO *io = virt_quad_io;
+ if (strcmp(name, "rc_throttle") == 0) {
+ set_shm(value, &io->rc_receiver[THROTTLE], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_roll") == 0) {
+ set_shm(value, &io->rc_receiver[ROLL], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_pitch") == 0) {
+ set_shm(value, &io->rc_receiver[PITCH], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_yaw") == 0) {
+ set_shm(value, &io->rc_receiver[YAW], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_gear") == 0) {
+ set_shm(value, &io->rc_receiver[GEAR], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_flap") == 0) {
+ set_shm(value, &io->rc_receiver[FLAP], &io->rc_lock);
+ }
+ else {
+ puts("Given input not recognized.");
+ ret = -1;
+ }
+ }
+ return ret;
+}
+
+void set_shm(float value, float *dest, pthread_mutex_t *lock) {
+ pthread_mutex_lock(lock);
+ *dest = value;
+ pthread_mutex_unlock(lock);
+}