diff --git a/Readme.md b/Readme.md
index a7949d3dd34199adac9d7828a08255af73be7042..c0f96eb671fa47dd7f9a8757a349847b79f247e7 100644
--- a/Readme.md
+++ b/Readme.md
@@ -9,16 +9,16 @@ Since 1998, MicroCART has been building aerial robots. Currently, we are
building a quadcopter that can fly autonomously.
MicroCART has 3 areas of development:
-- The quadcopter itself
+- The **quadcopter** itself
- The quadcopter has been built from the ground up, incorporating a
Zybo board that provides the processor and a FPGA, an IMU sensor, motors,
props, a LiPo battery, a receiver for manual remote control, and a frame
that holds it all together.
-- The ground station
+- The **ground station**
- The ground station is responsible for issuing important data to the quad
(like position data from the camera system) and issuing commands to the quad
for things like configuration and path following directives.
-- The Controls Model
+- The **controls model**
- The quadcopter processor is programmed to implement a PID controller. We use
a Simulink model to streamline the PID tuning process and to facilitate
effective characterization of the quad.
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/documentation/ci_faq.md b/documentation/ci_faq.md
index 679e0b9f484603505d1e64bd4c57369b255becc7..e630d3c166459bf3a85060f3405cb982a9302a75 100644
--- a/documentation/ci_faq.md
+++ b/documentation/ci_faq.md
@@ -16,7 +16,7 @@ condition.
1. Re-run the build.
- Sometimes something weird happens, so it's good to check this.
2. Figure out what you did wrong and fix it.
- - If you click on the build, you should be able to see the exact logs the
+ - If you click on the build, you should be able to see the exact logs of the
build process. Debug the error to figure out what went wrong. Maybe you
failed a test. You may need to open the source code for the test to see
where you failed, which is very insightful into figuring out what went
diff --git a/groundStation/auto_comp_graph.png b/groundStation/auto_comp_graph.png
new file mode 100644
index 0000000000000000000000000000000000000000..c124f1facf69bb24166793d6dee0dd2383db701d
Binary files /dev/null and b/groundStation/auto_comp_graph.png differ
diff --git a/groundStation/gui/MicroCART/MicroCART.pro b/groundStation/gui/MicroCART/MicroCART.pro
index e650809dadcff4eea45dd1649c1c3072d45ffecb..f8aa76c36c28c220e464900187f77d1a8b9225bf 100644
--- a/groundStation/gui/MicroCART/MicroCART.pro
+++ b/groundStation/gui/MicroCART/MicroCART.pro
@@ -18,12 +18,14 @@ SOURCES += main.cpp\
mainwindow.cpp \
wrappers.c \
trackerworker.cpp \
- controlworker.cpp
+ controlworker.cpp \
+ quaditem.cpp
HEADERS += mainwindow.h \
wrappers.h \
trackerworker.h \
- controlworker.h
+ controlworker.h \
+ quaditem.h
FORMS += mainwindow.ui
@@ -49,3 +51,6 @@ else:win32-g++:CONFIG(debug, debug|release): PRE_TARGETDEPS += $$PWD/../../../qu
else:win32:!win32-g++:CONFIG(release, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/release/computation_graph.lib
else:win32:!win32-g++:CONFIG(debug, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/debug/computation_graph.lib
else:unix: PRE_TARGETDEPS += $$PWD/../../../quad/lib/libcomputation_graph.a
+
+RESOURCES += \
+ resources.qrc
diff --git a/groundStation/gui/MicroCART/mainwindow.cpp b/groundStation/gui/MicroCART/mainwindow.cpp
index 34726c796af409a08e97a56651348ad70364d83a..22df6f238dd90b4a8764dbab5bdbfc1da82cec30 100644
--- a/groundStation/gui/MicroCART/mainwindow.cpp
+++ b/groundStation/gui/MicroCART/mainwindow.cpp
@@ -11,8 +11,8 @@
#include "wrappers.h"
#include "trackerworker.h"
#include "controlworker.h"
-
#include "graph_blocks.h"
+#include "quaditem.h"
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
@@ -27,8 +27,14 @@ MainWindow::MainWindow(QWidget *parent) :
{
ui->setupUi(this);
+ QGraphicsScene *topScene = new QGraphicsScene(this);
+ QuadItem * quad = new QuadItem();
+ ui->topView->setScene(topScene);
+
+ topScene->addItem(quad);
+
/* Set up environment variables */
- findChild<QLineEdit *>("socketPath")->setText(QProcessEnvironment::systemEnvironment().value("UCART_SOCKET"));
+ ui->socketPath->setText(QProcessEnvironment::systemEnvironment().value("UCART_SOCKET"));
/* Create a thread for workers */
QThread* workerThread = new QThread(this);
@@ -44,6 +50,9 @@ MainWindow::MainWindow(QWidget *parent) :
connect(trackerWorker, SIGNAL (finished(float, float, float, float, float, float)),
this, SLOT (updateTracker(float, float, float, float, float, float)));
+ connect(trackerWorker, SIGNAL (finished(float, float, float, float, float, float)),
+ quad, SLOT (quad->updateQuad(float, float, float, float, float, float)));
+
/* Create another worker for the control graph */
QThread * cwThread = new QThread(this);
ControlWorker * controlWorker = new ControlWorker();
@@ -58,8 +67,8 @@ MainWindow::MainWindow(QWidget *parent) :
connect(controlWorker, SIGNAL (paramSet(QString, QString)), controlWorker, SLOT (getParamValue(QString, QString)));
/* Signals to control worker */
- connect(findChild<QPushButton *>("pbControlRefresh"), SIGNAL (clicked()), controlWorker, SLOT (getNodes()));
- connect(findChild<QComboBox *>("nodeSelect"), SIGNAL (currentIndexChanged(QString)), controlWorker, SLOT (getParams(QString)));
+ connect(ui->pbControlRefresh, SIGNAL (clicked()), controlWorker, SLOT (getNodes()));
+ connect(ui->nodeSelect, SIGNAL (currentIndexChanged(QString)), controlWorker, SLOT (getParams(QString)));
connect(this, SIGNAL (getParamValue(QString, QString)), controlWorker, SLOT (getParamValue(QString, QString)));
connect(this, SIGNAL (setParamValue(QString, QString, float)), controlWorker, SLOT (setParamValue(QString, QString, float)));
@@ -72,7 +81,7 @@ MainWindow::MainWindow(QWidget *parent) :
/* Connect refresh button and refresh timer to tracker worker */
QTimer * trackerTimer = new QTimer(this);
connect(trackerTimer, SIGNAL(timeout()), trackerWorker, SLOT(process()));
- connect(findChild<QPushButton *>("pbRefresh"), SIGNAL (clicked()), trackerWorker, SLOT (process()));
+ connect(ui->pbRefresh, SIGNAL (clicked()), trackerWorker, SLOT (process()));
/* Timer used for next setpoint */
nextSpTimer->setSingleShot(true);
@@ -84,15 +93,15 @@ MainWindow::MainWindow(QWidget *parent) :
cwThread->start();
/* Connect the setpointlist to the model */
- findChild<QListView *>("setpointList")->setModel(setpointList);
+ ui->setpointList->setModel(setpointList);
/* Connect various things that can result in sending setpoints */
- connect(findChild<QPushButton *>("pbSendSetpoint"), SIGNAL (clicked()), this, SLOT (sendSetpoints()));
- connect(findChild<QLineEdit *>("xSetpoint"), SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
- connect(findChild<QLineEdit *>("ySetpoint"), SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
- connect(findChild<QLineEdit *>("zSetpoint"), SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
+ connect(ui->pbSendSetpoint, SIGNAL (clicked()), this, SLOT (sendSetpoints()));
+ connect(ui->xSetpoint, SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
+ connect(ui->ySetpoint, SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
+ connect(ui->zSetpoint, SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
- connect(findChild<QListView *>("setpointList"), SIGNAL (doubleClicked(QModelIndex)), this, SLOT (sendSelectedSetpoint()));
+ connect(ui->setpointList, SIGNAL (doubleClicked(QModelIndex)), this, SLOT (sendSelectedSetpoint()));
}
MainWindow::~MainWindow()
@@ -107,7 +116,7 @@ void MainWindow::updateConsole()
size_t len = 0;
len = readBackend(backendPipe, buf, len);
if (len > 0) {
- QLineEdit * con = findChild<QLineEdit *>("vConsole");
+ QLineEdit * con = ui->vConsole;
con->setText(con->text().append(buf));
}
}
@@ -115,38 +124,38 @@ void MainWindow::updateConsole()
void MainWindow::updateTracker(float x, float y, float z, float p, float r, float yaw)
{
- findChild<QLineEdit *>("xActual")->setText(QString::number(x));
- findChild<QLineEdit *>("yActual")->setText(QString::number(y));
- findChild<QLineEdit *>("zActual")->setText(QString::number(z));
- findChild<QLineEdit *>("pitchActual")->setText(QString::number(p));
- findChild<QLineEdit *>("rollActual")->setText(QString::number(r));
- findChild<QLineEdit *>("yawActual")->setText(QString::number(yaw));
+ ui->xActual->setText(QString::number(x));
+ ui->yActual->setText(QString::number(y));
+ ui->zActual->setText(QString::number(z));
+ ui->pitchActual->setText(QString::number(p));
+ ui->rollActual->setText(QString::number(r));
+ ui->yawActual->setText(QString::number(yaw));
float dist = sqrt(pow(x - sp_x, 2.0) + pow(y - sp_y, 2.0) + pow(z - sp_z, 2.0));
- findChild<QLineEdit *>("dist")->setText(QString::number(dist));
+ ui->dist->setText(QString::number(dist));
- if (!nextSpTimer->isActive() && findChild<QCheckBox *>("autonavEnabled")->isChecked() &&
- (dist < findChild<QLineEdit *>("autonavThreshold")->text().toFloat())) {
- nextSpTimer->start(findChild<QLineEdit *>("autonavDelay")->text().toInt());
+ if (!nextSpTimer->isActive() && ui->autonavEnabled->isChecked() &&
+ (dist < ui->autonavThreshold->text().toFloat())) {
+ nextSpTimer->start(ui->autonavDelay->text().toInt());
}
}
void MainWindow::on_pbStart_clicked()
{
- QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", findChild<QLineEdit *>("socketPath")->text());
- this->backendPid = startBackend(findChild<QLineEdit *>("backendPath")->text().toStdString().c_str(), &backendPipe);
- findChild<QPushButton *>("pbStart")->setEnabled(false);
- findChild<QPushButton *>("pbStop")->setEnabled(true);
+ QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", ui->socketPath->text());
+ this->backendPid = startBackend(ui->backendPath_2->text().toStdString().c_str(), &backendPipe);
+ ui->pbStart->setEnabled(false);
+ ui->pbStop->setEnabled(true);
backendState = 1;
}
void MainWindow::on_pbConnect_clicked()
{
- QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", findChild<QLineEdit *>("socketPath")->text());
- findChild<QPushButton *>("pbStart")->setEnabled(false);
- findChild<QPushButton *>("pbConnect")->setEnabled(false);
- findChild<QPushButton *>("pbStop")->setEnabled(true);
+ QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", ui->socketPath->text());
+ ui->pbStart->setEnabled(false);
+ ui->pbConnect->setEnabled(false);
+ ui->pbStop->setEnabled(true);
emit(connectWorkers());
backendState = 1;
}
@@ -159,9 +168,9 @@ void MainWindow::on_pbStop_clicked()
backendPipe = -1;
backendPid = 0;
}
- findChild<QPushButton *>("pbStart")->setEnabled(true);
- findChild<QPushButton *>("pbConnect")->setEnabled(true);
- findChild<QPushButton *>("pbStop")->setEnabled(false);
+ ui->pbStart->setEnabled(true);
+ ui->pbConnect->setEnabled(true);
+ ui->pbStop->setEnabled(false);
backendState = 0;
}
@@ -169,33 +178,33 @@ void MainWindow::on_chooseBackend_clicked()
{
QString backendPath = QFileDialog::getOpenFileName(this,
tr("Path to Backend Executable"));
- findChild<QLineEdit *>("backendPath")->setText(backendPath);
+ ui->backendPath_2->setText(backendPath);
}
void MainWindow::newNodes(QStringList blocks)
{
- QComboBox * select = findChild<QComboBox *>("nodeSelect");
+ QComboBox * select = ui->nodeSelect;
select->clear();
select->addItems(blocks);
- this->findChild<QLabel *>("noGraphWarning1")->setVisible(false);
- this->findChild<QLabel *>("noGraphWarning2")->setVisible(false);
- this->findChild<QWidget *>("noGraphWarningLine")->setVisible(false);
+ this->ui->noGraphWarning1->setVisible(false);
+ this->ui->noGraphWarning2->setVisible(false);
+ this->ui->noGraphWarningLine->setVisible(false);
}
void MainWindow::newConstantBlocks(QStringList blocks)
{
- QComboBox * xSelect = findChild<QComboBox *>("xSetpointSelect");
+ QComboBox * xSelect = ui->xSetpointSelect;
xSelect->clear();
xSelect->addItems(blocks);
- QComboBox * ySelect = findChild<QComboBox *>("ySetpointSelect");
+ QComboBox * ySelect = ui->ySetpointSelect;
ySelect->clear();
ySelect->addItems(blocks);
- QComboBox * zSelect = findChild<QComboBox *>("zSetpointSelect");
+ QComboBox * zSelect = ui->zSetpointSelect;
zSelect->clear();
zSelect->addItems(blocks);
@@ -216,64 +225,64 @@ void MainWindow::newConstantBlocks(QStringList blocks)
void MainWindow::newParams(QStringList params)
{
- QComboBox * select = findChild<QComboBox *>("paramSelect");
+ QComboBox * select = ui->paramSelect;
select->clear();
select->addItems(params);
}
void MainWindow::newParamValue(QString node, QString param, float val)
{
- findChild<QLineEdit *>("paramValue")->setText(QString::number(val));
+ ui->paramValue->setText(QString::number(val));
/* Update the nav page setpoints if it's a setpoint paramvalue */
- if (node == findChild<QComboBox *>("xSetpointSelect")->currentText()) {
- findChild<QLineEdit *>("xSetpoint")->setText(QString::number(val));
- } else if (node == findChild<QComboBox *>("ySetpointSelect")->currentText()) {
- findChild<QLineEdit *>("ySetpoint")->setText(QString::number(val));
- } else if (node == findChild<QComboBox *>("zSetpointSelect")->currentText()) {
- findChild<QLineEdit *>("zSetpoint")->setText(QString::number(val));
+ if (node == ui->xSetpointSelect->currentText()) {
+ ui->xSetpoint->setText(QString::number(val));
+ } else if (node == ui->ySetpointSelect->currentText()) {
+ ui->ySetpoint->setText(QString::number(val));
+ } else if (node == ui->zSetpointSelect->currentText()) {
+ ui->zSetpoint->setText(QString::number(val));
}
}
void MainWindow::on_paramSelect_currentIndexChanged(const QString &arg1)
{
- emit(getParamValue(findChild<QComboBox *>("nodeSelect")->currentText(), arg1));
+ emit(getParamValue(ui->nodeSelect->currentText(), arg1));
}
void MainWindow::on_paramValue_returnPressed()
{
- emit (setParamValue(findChild<QComboBox *>("nodeSelect")->currentText(),
- findChild<QComboBox *>("paramSelect")->currentText(),
- findChild<QLineEdit *>("paramValue")->text().toFloat()));
+ emit (setParamValue(ui->nodeSelect->currentText(),
+ ui->paramSelect->currentText(),
+ ui->paramValue->text().toFloat()));
}
void MainWindow::sendSetpoints()
{
- sp_x = findChild<QLineEdit *>("xSetpoint")->text().toFloat();
- emit (setParamValue(findChild<QComboBox *>("xSetpointSelect")->currentText(),
+ sp_x = ui->xSetpoint->text().toFloat();
+ emit (setParamValue(ui->xSetpointSelect->currentText(),
blockDefs[BLOCK_CONSTANT]->param_names[0], sp_x));
- sp_y = findChild<QLineEdit *>("ySetpoint")->text().toFloat();
- emit (setParamValue(findChild<QComboBox *>("ySetpointSelect")->currentText(),
+ sp_y = ui->ySetpoint->text().toFloat();
+ emit (setParamValue(ui->ySetpointSelect->currentText(),
blockDefs[BLOCK_CONSTANT]->param_names[0], sp_y));
- sp_z = findChild<QLineEdit *>("zSetpoint")->text().toFloat();
- emit (setParamValue(findChild<QComboBox *>("zSetpointSelect")->currentText(),
+ sp_z = ui->zSetpoint->text().toFloat();
+ emit (setParamValue(ui->zSetpointSelect->currentText(),
blockDefs[BLOCK_CONSTANT]->param_names[0], sp_z));
}
void MainWindow::on_pbAppendSetpoint_clicked()
{
- QString str("[" + findChild<QLineEdit *>("xSetpoint")->text() + ", "+
- findChild<QLineEdit *>("ySetpoint")->text() + ", " +
- findChild<QLineEdit *>("zSetpoint")->text() + "]");
+ QString str("[" + ui->xSetpoint->text() + ", "+
+ ui->ySetpoint->text() + ", " +
+ ui->zSetpoint->text() + "]");
setpointList->appendRow(new QStandardItem(str));
}
void MainWindow::on_pbNextSetpoint_clicked()
{
- QListView * listView = findChild<QListView *>("setpointList");
+ QListView * listView = ui->setpointList;
if (listView->currentIndex().isValid() && setpointList->index(listView->currentIndex().row() + 1, 0).isValid()) {
listView->setCurrentIndex(setpointList->index(listView->currentIndex().row() + 1, 0));
} else {
@@ -285,14 +294,14 @@ void MainWindow::on_pbNextSetpoint_clicked()
void MainWindow::sendSelectedSetpoint()
{
- if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
+ if (ui->setpointList->currentIndex().isValid()) {
QRegExp regex("\\[(.*), (.*), (.*)\\]");
- int row = findChild<QListView *>("setpointList")->currentIndex().row();
+ int row = ui->setpointList->currentIndex().row();
regex.indexIn(setpointList->item(row)->text());
- findChild<QLineEdit *>("xSetpoint")->setText(regex.cap(1));
- findChild<QLineEdit *>("ySetpoint")->setText(regex.cap(2));
- findChild<QLineEdit *>("zSetpoint")->setText(regex.cap(3));
+ ui->xSetpoint->setText(regex.cap(1));
+ ui->ySetpoint->setText(regex.cap(2));
+ ui->zSetpoint->setText(regex.cap(3));
sendSetpoints();
}
@@ -300,36 +309,36 @@ void MainWindow::sendSelectedSetpoint()
void MainWindow::on_pbActualToSetpoint_clicked()
{
- findChild<QLineEdit *>("xSetpoint")->setText(findChild<QLineEdit *>("xActual")->text());
- findChild<QLineEdit *>("ySetpoint")->setText(findChild<QLineEdit *>("yActual")->text());
- findChild<QLineEdit *>("zSetpoint")->setText(findChild<QLineEdit *>("zActual")->text());
+ ui->xSetpoint->setText(ui->xActual->text());
+ ui->ySetpoint->setText(ui->yActual->text());
+ ui->zSetpoint->setText(ui->zActual->text());
}
void MainWindow::on_pbDeleteSetpoint_clicked()
{
- if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
- setpointList->removeRow(findChild<QListView *>("setpointList")->currentIndex().row());
+ if (ui->setpointList->currentIndex().isValid()) {
+ setpointList->removeRow(ui->setpointList->currentIndex().row());
}
}
void MainWindow::newControlGraph(QString graph)
{
- findChild<QLabel *>("graphImage")->setPixmap(QPixmap(graph));
+ ui->graphImage->setPixmap(QPixmap(graph));
}
void MainWindow::on_pbActualToWaypoint_clicked()
{
- QString str("[" + findChild<QLineEdit *>("xActual")->text() + ", "+
- findChild<QLineEdit *>("yActual")->text() + ", " +
- findChild<QLineEdit *>("zActual")->text() + "]");
+ QString str("[" + ui->xActual->text() + ", "+
+ ui->yActual->text() + ", " +
+ ui->zActual->text() + "]");
setpointList->appendRow(new QStandardItem(str));
}
void MainWindow::on_pbMoveUp_clicked()
{
- if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
- int current = findChild<QListView *>("setpointList")->currentIndex().row();
+ if (ui->setpointList->currentIndex().isValid()) {
+ int current = ui->setpointList->currentIndex().row();
if (current > 0) {
setpointList->insertRow(current - 1, setpointList->takeItem(current));
setpointList->removeRow(current + 1);
@@ -339,8 +348,8 @@ void MainWindow::on_pbMoveUp_clicked()
void MainWindow::on_pbMoveDown_clicked()
{
- if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
- int current = findChild<QListView *>("setpointList")->currentIndex().row();
+ if (ui->setpointList->currentIndex().isValid()) {
+ int current = ui->setpointList->currentIndex().row();
if (current < (setpointList->rowCount() - 1)) {
setpointList->insertRow(current + 2, setpointList->takeItem(current));
setpointList->removeRow(current);
@@ -351,13 +360,13 @@ void MainWindow::on_pbMoveDown_clicked()
void MainWindow::on_pbInsertSetpoint_clicked()
{
int current = 0;
- if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
- current = findChild<QListView *>("setpointList")->currentIndex().row();
+ if (ui->setpointList->currentIndex().isValid()) {
+ current = ui->setpointList->currentIndex().row();
}
- QString str("[" + findChild<QLineEdit *>("xSetpoint")->text() + ", "+
- findChild<QLineEdit *>("ySetpoint")->text() + ", " +
- findChild<QLineEdit *>("zSetpoint")->text() + "]");
+ QString str("[" + ui->xSetpoint->text() + ", "+
+ ui->ySetpoint->text() + ", " +
+ ui->zSetpoint->text() + "]");
setpointList->insertRow(current, new QStandardItem(str));
}
diff --git a/groundStation/gui/MicroCART/mainwindow.h b/groundStation/gui/MicroCART/mainwindow.h
index abdbd1d3d473d522203a8fe1df0d27f7592f3e44..de7b2a32dc5e3ec0f0099403a676644a0fa8666a 100644
--- a/groundStation/gui/MicroCART/mainwindow.h
+++ b/groundStation/gui/MicroCART/mainwindow.h
@@ -4,6 +4,8 @@
#include <QMainWindow>
#include <QStringList>
#include <QStandardItemModel>
+#include <QGraphicsScene>
+#include "quaditem.h"
namespace Ui {
class MainWindow;
diff --git a/groundStation/gui/MicroCART/mainwindow.ui b/groundStation/gui/MicroCART/mainwindow.ui
index 829e4f0726064b51fa3d65781def6ccb79ab34b0..16c1d1a5e6a135a79cabc2af4735569d2cb5b4f8 100644
--- a/groundStation/gui/MicroCART/mainwindow.ui
+++ b/groundStation/gui/MicroCART/mainwindow.ui
@@ -18,7 +18,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>0</number>
+ <number>2</number>
</property>
<widget class="QWidget" name="backend">
<attribute name="title">
@@ -151,8 +151,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>968</width>
- <height>666</height>
+ <width>962</width>
+ <height>660</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_8">
@@ -842,17 +842,21 @@
</layout>
</item>
<item>
- <spacer name="horizontalSpacer_3">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
+ <layout class="QVBoxLayout" name="verticalLayout_9">
+ <item>
+ <widget class="QGraphicsView" name="topView">
+ <property name="verticalScrollBarPolicy">
+ <enum>Qt::ScrollBarAlwaysOff</enum>
+ </property>
+ <property name="horizontalScrollBarPolicy">
+ <enum>Qt::ScrollBarAlwaysOff</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGraphicsView" name="sideView"/>
+ </item>
+ </layout>
</item>
</layout>
</item>
@@ -875,7 +879,7 @@
<x>0</x>
<y>0</y>
<width>1004</width>
- <height>30</height>
+ <height>27</height>
</rect>
</property>
</widget>
diff --git a/groundStation/gui/MicroCART/quad.png b/groundStation/gui/MicroCART/quad.png
new file mode 100644
index 0000000000000000000000000000000000000000..1dd4ab2151f5b67d9477029f0ea70afeb8acdbf5
Binary files /dev/null and b/groundStation/gui/MicroCART/quad.png differ
diff --git a/groundStation/gui/MicroCART/quaditem.cpp b/groundStation/gui/MicroCART/quaditem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b43ced998e8720bf1cb7d50823a6bc8f39b3bef5
--- /dev/null
+++ b/groundStation/gui/MicroCART/quaditem.cpp
@@ -0,0 +1,32 @@
+#include "quaditem.h"
+#include <QPixmap>
+#include <QLabel>
+
+QuadItem::QuadItem()
+{
+ setFlag(ItemIsMovable);
+}
+
+QRectF QuadItem::boundingRect() const
+{
+ return QRectF(0,0,50,50);
+}
+
+void QuadItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
+{
+ QRectF rec = boundingRect();
+ painter->drawImage(rec,QImage(":/images/quad/quad.png"));
+}
+
+void QuadItem::updateQuad(float longitudinal, float lateral, float height, float pitch, float roll, float yaw)
+{
+ this->setPos(longitudinal*10, lateral*10);
+
+ QPointF center = boundingRect().center();
+ QTransform trans = this->transform();
+ trans.translate(center.x(), center.y());
+ trans.rotate(yaw);
+ trans.translate(-center.x(), -center.y());
+ this->setTransform(trans);
+ update();
+}
diff --git a/groundStation/gui/MicroCART/quaditem.h b/groundStation/gui/MicroCART/quaditem.h
new file mode 100644
index 0000000000000000000000000000000000000000..0688802193e5e88b83c3507f48c7674b69d2434e
--- /dev/null
+++ b/groundStation/gui/MicroCART/quaditem.h
@@ -0,0 +1,25 @@
+#ifndef QUADITEM_H
+#define QUADITEM_H
+
+#include <QPainter>
+#include <QGraphicsItem>
+#include <QPixmap>
+#include <QLabel>
+#include <QtCore>
+#include <QtGui>
+#include <QTransform>
+
+class QuadItem : public QObject, public QGraphicsItem
+{
+ Q_OBJECT
+public:
+ QuadItem();
+ QRectF boundingRect() const;
+ void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget);
+
+private slots:
+ void updateQuad(float longitudinal, float lateral, float height, float pitch, float roll, float yaw);
+
+};
+
+#endif // QUADITEM_H
diff --git a/groundStation/gui/MicroCART/resources.qrc b/groundStation/gui/MicroCART/resources.qrc
new file mode 100644
index 0000000000000000000000000000000000000000..72e37cba5e4e61fd619ef180224985ad27ab3044
--- /dev/null
+++ b/groundStation/gui/MicroCART/resources.qrc
@@ -0,0 +1,5 @@
+<RCC>
+ <qresource prefix="/images/quad">
+ <file>quad.png</file>
+ </qresource>
+</RCC>
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/README.md b/quad/README.md
index 298602985dab7218993638e4c3aaa1ec1779df7e..4723b848d6509afe038a9051f026f32b6c5403b7 100644
--- a/quad/README.md
+++ b/quad/README.md
@@ -64,6 +64,12 @@ hardware interface.
Look in `xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c` for instructions.
Ideally, you would run these tests from the XSDK IDE.
+### XSDK FYIs
+The XIlinx SDK has a few quirks that are important to watch out for:
+ 1. From the [documentation](https://www.xilinx.com/support/documentation/sw_manuals/xilinx14_7/SDK_Doc/tasks/sdk_t_tcf_limitations_faq.htm), if you abort program execution while at a breakpoint inside an interrupt handler, when re-running the program, interrupts don’t fire. You have to do a hard reset of the board (cycle power) to have interrupts work again.
+ 2. After doing a `git pull`, refresh the files by right-clicking on the project in the sidebar and clicking "Refresh"
+ 3. The project does not detect changes in header files, so if you modify a `.h` file, you should do a clean before building, otherwise you may experience unexpected behavior.
+
### Quad Stats
Optical flow sensor is offset by 35.64 degrees.
diff --git a/quad/executable.mk b/quad/executable.mk
index 0666c88a7cb8887a726c5e7717c2eaab61ab67c7..1662310560afd5b81b0fc73bbd83ec29155ab135 100644
--- a/quad/executable.mk
+++ b/quad/executable.mk
@@ -12,7 +12,7 @@ OBJECTS = $(patsubst %.c, $(OBJDIR)/%.o, $(SOURCES))
TARGET = $(EXEDIR)/$(NAME)
-CLEANUP = $(TARGET) $(OBJDIR)
+CLEANUP = $(TARGET) $(OBJDIR) $(NAME)
.PHONY: default run clean
@@ -22,9 +22,6 @@ CLEANUP = $(TARGET) $(OBJDIR)
default: $(TARGET)
-run: $(TARGET)
- $(EXEDIR)/$(NAME)
-
clean:
rm -rf $(CLEANUP)
@@ -34,6 +31,7 @@ clean:
$(TARGET): $(OBJECTS) | $(EXEDIR)
$(GCC) -g -o $(TARGET) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)
+ cp $(TARGET) $(NAME)
$(OBJDIR)/%.o : %.c | $(OBJDIR) $(INCDIR)
$(GCC) -c -g -o $@ $< -I$(INCDIR) -Wall
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index 9e7d693e6ee8459bedf24cc503a6265dcd8e6db0..20c49fb33a75780419aa360794e8d3a757473766 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -12,32 +12,36 @@ bin_dir = script_dir + "/../../bin/"
Dir.chdir(bin_dir)
-Timeout::timeout(30) {
+Timeout::timeout(60) {
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)
+
+ # Set RC switches
+ set_gear GEAR_OFF
+ set_flap FLAP_ON
+
+ # Set initial quad orientation (flat on the ground, facing forward)
+ `./virt-quad set i2c_imu_x 0`
+ `./virt-quad set i2c_imu_y 0`
+ `./virt-quad set i2c_imu_z -1`
+ `./virt-quad set rc_roll 0.498`
+ `./virt-quad set rc_pitch 0.497`
+ `./virt-quad set rc_yaw 0.498`
#################
# Begin Tests
#################
begin
-
puts("Beginning tests...")
- # Set gravity and gear
- File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
- File.write(GEAR, GEAR_OFF)
-
puts("Check that motors are off at startup")
check_motors_are_off "Motors weren't off at startup! How dangerous!"
@@ -47,7 +51,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 +59,87 @@ 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, 3)
+ spinner.exit
+ p motors
+ motors.each { |value| assert_operator(value, :>, THROTTLE_QUAR) }
+
+ puts("Check that when quad is tilted, motors respond correctly")
+ puts("Tilting forwards...")
+ `./virt-quad set i2c_imu_x 0.25`
+ spinner = Thread.new { delay_spin_cursor(5) }
+ motors = get_motor_averages(100, 3)
+ spinner.exit
+ p motors
+ assert_operator(motors[0], :>, motors[1])
+ assert_operator(motors[0], :>, motors[3])
+ assert_operator(motors[2], :>, motors[1])
+ assert_operator(motors[2], :>, motors[3])
+ puts("Tilting backwards...")
+ `./virt-quad set i2c_imu_x -0.25`
+ spinner = Thread.new { delay_spin_cursor(5) }
+ motors = get_motor_averages(100, 3)
+ spinner.exit
+ p motors
+ assert_operator(motors[0], :<, motors[1])
+ assert_operator(motors[0], :<, motors[3])
+ assert_operator(motors[2], :<, motors[1])
+ assert_operator(motors[2], :<, motors[3])
+ puts("Tilting right...")
+ `./virt-quad set i2c_imu_x 0`
+ `./virt-quad set i2c_imu_y 0.25`
+ spinner = Thread.new { delay_spin_cursor(5) }
+ motors = get_motor_averages(100, 3)
+ spinner.exit
+ p motors
+ assert_operator(motors[0], :<, motors[2])
+ assert_operator(motors[0], :<, motors[3])
+ assert_operator(motors[1], :<, motors[2])
+ assert_operator(motors[1], :<, motors[3])
+ puts("Tilting left...")
+ `./virt-quad set i2c_imu_y -0.25`
+ spinner = Thread.new { delay_spin_cursor(5) }
+ motors = get_motor_averages(100, 3)
+ spinner.exit
+ p motors
+ assert_operator(motors[0], :>, motors[2])
+ assert_operator(motors[0], :>, motors[3])
+ assert_operator(motors[1], :>, motors[2])
+ assert_operator(motors[1], :>, motors[3])
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..6301d840d821f51356b1df742f7a1490fff98fb5 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,49 @@ require 'timeout'
require 'thread'
include Test::Unit::Assertions
-$fifos = Hash.new
+# Utility functions
+def check_motors_are_off(msg)
+ motors = [ ]
+ motors.push(`./virt-quad get motor1`.to_f)
+ motors.push(`./virt-quad get motor2`.to_f)
+ motors.push(`./virt-quad get motor3`.to_f)
+ motors.push(`./virt-quad get motor4`.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 = [ ]
+ motors.push(`./virt-quad get motor1`.to_f)
+ motors.push(`./virt-quad get motor2`.to_f)
+ motors.push(`./virt-quad get motor3`.to_f)
+ motors.push(`./virt-quad get motor4`.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/graph_blocks/node_mixer.c b/quad/src/graph_blocks/node_mixer.c
index 046f133eefde61de324f61d805f5dbc97616c664..f1baea798cf5b741d56ba2fb0ff98991b76d5fb3 100644
--- a/quad/src/graph_blocks/node_mixer.c
+++ b/quad/src/graph_blocks/node_mixer.c
@@ -1,12 +1,14 @@
#include "node_mixer.h"
#include <stdlib.h>
+#include <math.h>
static int motor_min = 0.00000;
static int motor_max = 1.00000;
static double motor_clamp(double val) {
- if (val < motor_min) {val = motor_min;}
- if (val > motor_max) {val = motor_max;}
+ if (isnan(val)) { val = motor_min; }
+ else if (val < motor_min) {val = motor_min;}
+ else if (val > motor_max) {val = motor_max;}
return val;
}
diff --git a/quad/src/quad_app/controllers.c b/quad/src/quad_app/controllers.c
index 1ebf713e3e101065e22086b816eca9454600160b..14abde800f5ea36af1154894c29fdf18e680067f 100644
--- a/quad/src/quad_app/controllers.c
+++ b/quad/src/quad_app/controllers.c
@@ -9,7 +9,6 @@
* Lots of useful information in controllers.h, look in there first
*/
#include "controllers.h"
-#include "iic_utils.h"
#include "quadposition.h"
#include "util.h"
#include "stdio.h"
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index ca787b9fc6dde55ce2cddb69d8c77036aee41cd1..03b8550822b6d87b60bedcb44761f4d7673ed394 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/quad/src/quad_app/hw_iface.h
@@ -15,18 +15,10 @@
* ../virt_quad -> running quad_app in a Unix environment
*/
-struct I2CDriver {
- void *state;
- int (*reset)(struct I2CDriver *self);
- int (*write)(struct I2CDriver *self,
- unsigned short device_addr,
- unsigned char *data,
- unsigned int length);
- int (*read)(struct I2CDriver *self,
- unsigned short device_addr,
- unsigned char *buff,
- unsigned int length);
-};
+// Forward declared types
+typedef struct gam gam_t;
+typedef struct lidar lidar_t;
+typedef struct px4flow px4flow_t;
struct RCReceiverDriver {
void *state;
@@ -67,4 +59,35 @@ struct SystemDriver {
int (*sleep)(struct SystemDriver *self, unsigned long us);
};
+struct I2CDriver {
+ void *state;
+ int (*reset)(struct I2CDriver *self);
+ int (*write)(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *data,
+ unsigned int length);
+ int (*read)(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *buff,
+ unsigned int length);
+};
+
+struct IMUDriver {
+ struct I2CDriver *i2c;
+ int (*reset)(struct IMUDriver *self);
+ int (*read)(struct IMUDriver *self, gam_t *gam);
+};
+
+struct LidarDriver {
+ struct I2CDriver *i2c;
+ int (*reset)(struct LidarDriver *self);
+ int (*read)(struct LidarDriver *self, lidar_t *lidar);
+};
+
+struct OpticalFlowDriver {
+ struct I2CDriver *i2c;
+ int (*reset)(struct OpticalFlowDriver *self);
+ int (*read)(struct OpticalFlowDriver *self, px4flow_t *of);
+};
+
#endif
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
deleted file mode 100644
index 5674ac93bb4cd6c3ea202181a1c702c75015164e..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/iic_utils.c
+++ /dev/null
@@ -1,343 +0,0 @@
-/**
- * IIC_UTILS.c
- *
- * Utility functions for using I2C on a Diligent Zybo board.
- * Supports the SparkFun MPU9150 and the LiDAR lite v2 sensor
- */
-
-#include <stdio.h>
-#include <math.h>
-
-#include "iic_utils.h"
-
-
-static struct I2CDriver *i2c;
-static struct SystemDriver *sys;
-
-double magX_correction = -1, magY_correction, magZ_correction;
-
-void iic_set_globals(struct I2CDriver *given_i2c, struct SystemDriver *given_system) {
- i2c = given_i2c;
- sys = given_system;
-}
-
-int iic0_mpu9150_start(){
-
- // Device Reset & Wake up
- iic0_mpu9150_write(0x6B, 0x80);
- sys->sleep(sys, 5000);
-
- // Set clock reference to Z Gyro
- iic0_mpu9150_write(0x6B, 0x03);
- // Configure Digital Low/High Pass filter
- iic0_mpu9150_write(0x1A,0x03); // Level 3 low pass on gyroscope
-
- // Configure Gyro to 2000dps, Accel. to +/-8G
- iic0_mpu9150_write(0x1B, 0x18);
- iic0_mpu9150_write(0x1C, 0x10);
-
- // Enable I2C bypass for AUX I2C (Magnetometer)
- iic0_mpu9150_write(0x37, 0x02);
-
- // Setup Mag
- iic0_mpu9150_write(0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
-
- sys->sleep(sys, 100000);
-
- int i;
- gam_t temp_gam;
-
- // Do about 20 reads to warm up the device
- for(i=0; i < 20; ++i){
- if(iic0_mpu9150_read_gam(&temp_gam) == -1){
- return -1;
- }
- sys->sleep(sys, 1000);
- }
-
- return 0;
-}
-
-void iic0_mpu9150_stop(){
-
- //Put MPU to sleep
- iic0_mpu9150_write(0x6B, 0b01000000);
-}
-
-int iic0_mpu9150_write(u8 register_addr, u8 data){
-
- u16 device_addr = MPU9150_DEVICE_ADDR;
- u8 buf[] = {register_addr, data};
-
- // Check if within register range
- if(register_addr < 0 || register_addr > 0x75){
- return;
- }
-
- if(register_addr <= 0x12){
- device_addr = MPU9150_COMPASS_ADDR;
- }
-
- return i2c->write(i2c, device_addr, buf, 2);
-}
-
-int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
-
- u16 device_addr = MPU9150_DEVICE_ADDR;
- u8 buf[] = {register_addr};
-
- // Check if within register range
- if(register_addr < 0 || register_addr > 0x75){
- }
-
- // Set device address to the if 0x00 <= register address <= 0x12
- if(register_addr <= 0x12){
- device_addr = MPU9150_COMPASS_ADDR;
- }
-
-
- int status = i2c->write(i2c, device_addr, buf, 1);
- if (!status) {
- status = i2c->read(i2c, device_addr, recv_buffer, size);
- }
- return status;
-}
-
-void iic0_mpu9150_calc_mag_sensitivity(){
-
- u8 buf[3];
- u8 ASAX, ASAY, ASAZ;
-
- // Quickly read from the factory ROM to get correction coefficents
- iic0_mpu9150_write(0x0A, 0x0F);
- sys->sleep(sys, 10000);
-
- // Read raw adjustment values
- iic0_mpu9150_read(buf, 0x10,3);
- ASAX = buf[0];
- ASAY = buf[1];
- ASAZ = buf[2];
-
- // Set the correction coefficients
- magX_correction = (ASAX-128)*0.5/128 + 1;
- magY_correction = (ASAY-128)*0.5/128 + 1;
- magZ_correction = (ASAZ-128)*0.5/128 + 1;
-}
-
-
-void iic0_mpu9150_read_mag(gam_t* gam){
-
- u8 mag_data[6];
- i16 raw_magX, raw_magY, raw_magZ;
-
- // Grab calibrations if not done already
- if(magX_correction == -1){
- iic0_mpu9150_calc_mag_sensitivity();
- }
-
- // Set Mag to single read mode
- iic0_mpu9150_write(0x0A, 0x01);
- sys->sleep(sys, 10000);
- mag_data[0] = 0;
-
- // Keep checking if data is ready before reading new mag data
- while(mag_data[0] == 0x00){
- iic0_mpu9150_read(mag_data, 0x02, 1);
- }
-
- // Get mag data
- iic0_mpu9150_read(mag_data, 0x03, 6);
-
- raw_magX = (mag_data[1] << 8) | mag_data[0];
- raw_magY = (mag_data[3] << 8) | mag_data[2];
- raw_magZ = (mag_data[5] << 8) | mag_data[4];
-
- // Set magnetometer data to output
- gam->mag_x = raw_magX * magX_correction;
- gam->mag_y = raw_magY * magY_correction;
- gam->mag_z = raw_magZ * magZ_correction;
-
-}
-
-static int iic_rx_timeout_failures;
-int iic_get_gam_timeout_failures() {
- return iic_rx_timeout_failures;
-}
-static int iic_cons_timeout_failures;
-int iic_get_consecutive_gam_timeout_failures() {
- return iic_cons_timeout_failures;
-}
-
-/**
- * Get Gyro Accel Mag (GAM) information
- */
-int iic0_mpu9150_read_gam(gam_t* gam) {
-
- i16 raw_accel_x, raw_accel_y, raw_accel_z;
- i16 gyro_x, gyro_y, gyro_z;
-
- u8 sensor_data[ACCEL_GYRO_READ_SIZE] = {};
-
- // We should only get mag_data ~10Hz
- //Xint8 mag_data[6] = {};
-
- //readHandler = iic0_read_bytes(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
- int status = iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
- if (status == IIC_RX_TIMEOUT_FAILURE) {
- iic_rx_timeout_failures++;
- iic_cons_timeout_failures++;
- return status;
- } else if (status) {
- return status;
- }
- iic_cons_timeout_failures = 0; // Successful read
-
- //Calculate accelerometer data
- raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
- raw_accel_y = sensor_data[ACC_Y_H] << 8 | sensor_data[ACC_Y_L];
- raw_accel_z = sensor_data[ACC_Z_H] << 8 | sensor_data[ACC_Z_L];
-
- // put in G's
- gam->accel_x = (raw_accel_x / 4096.0) + ACCEL_X_BIAS; // 4,096 is the gain per LSB of the measurement reading based on a configuration range of +-8g
- gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
- gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
-
-
- //Convert gyro data to rate (we're only using the most 12 significant bits)
- gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
- gyro_y = (sensor_data[GYR_Y_H] << 8 | sensor_data[GYR_Y_L]);// * G_GAIN;
- gyro_z = (sensor_data[GYR_Z_H] << 8 | sensor_data[GYR_Z_L]);// * G_GAIN;
-
- //Get the number of degrees
- //javey: converted to radians to following SI units
- gam->gyro_xVel_p = ((gyro_x / GYRO_SENS) * DEG_TO_RAD) + GYRO_X_BIAS;
- gam->gyro_yVel_q = ((gyro_y / GYRO_SENS) * DEG_TO_RAD) + GYRO_Y_BIAS;
- gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
-
- return 0;
-}
-
-//////////////////////////////////////////////////
-// LIDAR
-//////////////////////////////////////////////////
-
-int iic0_lidarlite_write(u8 register_addr, u8 data) {
- u8 buf[] = {register_addr, data};
-
- return i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 2);
-}
-
-int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
- u8 buf[] = {register_addr};
- int status = 0;
-
- status |= i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
- // Only read if write succeeded
- if (status == 0) {
- status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
- }
- return status;
-}
-
-int iic0_lidarlite_init() {
- int status = 0;
-
- // Device Reset & Wake up with default settings
- status = iic0_lidarlite_write(0x00, 0x00);
- sys->sleep(sys, 15000);
-
- // Enable Free Running Mode and distance measurements with correction
- status |= iic0_lidarlite_write(0x11, 0xff);
- status |= iic0_lidarlite_write(0x00, 0x04);
-
- return status;
-}
-
-int iic0_lidarlite_sleep() {
- return iic0_lidarlite_write(0x65, 0x84);
-}
-
-int iic0_lidarlite_read_distance(lidar_t *lidar) {
- u8 buf[2];
- int status = 0;
-
- // Read the sensor value
- status = iic0_lidarlite_read(buf, 0x8f, 2);
- float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
- lidar->distance_m = 0.01 * distance_cm;
-
- return status;
-}
-
-//////////////////////////////////////////////////
-// PX4FLOW
-//////////////////////////////////////////////////
-
-//TODO: Replace device-specific iic0_device_write/read with generic ones
-
-int iic0_px4flow_write(u8 register_addr, u8 data) {
- u8 buf[] = {register_addr, data};
-
- return i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 2);
-}
-
-int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size) {
- u8 buf[] = {register_addr};
- int status = 0;
-
- status |= i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
- if (status == 0) {
- status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
- }
- return status;
-}
-
-int iic0_px4flow_init(px4flow_t *of) {
- //Clear struct
- of->xVel = 0;
- of->yVel = 0;
-
- //Perform initial update
- // TODO: Re-enable this initial read once virtual mock-up is made
- //return iic0_px4flow_update(of);
- return 0;
-}
-
-int iic0_px4flow_update(px4flow_t *of) {
- int status = 0;
-
- struct {
- uint16_t frameCount;
-
- int16_t pixelFlowXSum;
- int16_t pixelFlowYSum;
- int16_t flowCompX;
- int16_t flowCompY;
- int16_t qual;
-
- int16_t gyroXRate;
- int16_t gyroYRate;
- int16_t gyroZRate;
-
- uint8_t gyroRange;
- uint8_t sonarTimestamp;
- int16_t groundDistance;
- } i2cFrame;
-
- u8 buf[sizeof(i2cFrame)];
-
- // Read the sensor value
- status = iic0_px4flow_read(buf, 0x00, sizeof(i2cFrame));
-
- if(status == 0) {
- //Copy into struct
- memcpy(&i2cFrame, buf, sizeof(i2cFrame));
-
- of->xVel = i2cFrame.flowCompX / 1000.;
- of->yVel = i2cFrame.flowCompY / 1000.;
- of->quality = i2cFrame.qual;
- of->distance = i2cFrame.groundDistance / 1000.;
- }
-
- return status;
-}
diff --git a/quad/src/quad_app/iic_utils.h b/quad/src/quad_app/iic_utils.h
deleted file mode 100644
index c7f4bc0b8ee38b38620c98b1ee10ab57cf95f8db..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/iic_utils.h
+++ /dev/null
@@ -1,152 +0,0 @@
-/* iic_mpu9150_utils.h
- *
- * A header file for the prototyping constants used for
- * the I2C Controller 0 (I2C0) on the Zybo Board
- *
- * This file is intended SOLELY for the Sparkfun MPU9150
- * and the Diligent ZyBo Board
- *
- * Author: ucart
- *
- */
-
-
-#ifndef IIC_UTILS_H
-#define IIC_UTILS_H
-
-#include "type_def.h"
-#include "hw_iface.h"
-
-// System configuration registers
-// (Please see Appendix B: System Level Control Registers in the Zybo TRM)
-#define IIC_SYSTEM_CONTROLLER_RESET_REG_ADDR (0xF8000224)
-#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
-
-// IIC0 Registers
-#define IIC0_CONTROL_REG_ADDR (0xE0004000)
-#define IIC0_STATUS_REG_ADDR (0xE0004004)
-#define IIC0_SLAVE_ADDR_REG (0xE0004008)
-#define IIC0_DATA_REG_ADDR (0xE000400C)
-#define IIC0_INTR_STATUS_REG_ADDR (0xE0004010)
-#define IIC0_TRANFER_SIZE_REG_ADDR (0xE0004014)
-#define IIC0_INTR_EN (0xE0004024)
-#define IIC0_TIMEOUT_REG_ADDR (0xE000401C)
-
-//Interrupt Status Register Masks
-#define ARB_LOST (0x200)
-#define RX_UNF (0x80)
-#define TX_OVF (0x40)
-#define RX_OVF (0x20)
-#define SLV_RDY (0x10)
-#define TIME_OUT (0x08)
-#define NACK (0x04)
-#define MORE_DAT (0x02)
-#define TRANS_COMPLETE (0x01)
-
-#define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
-#define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
-
-// Error code could be returned on an iic read if the watchdog timer triggers
-#define IIC_RX_TIMEOUT_FAILURE (-88)
-
-///////////////////////////////////////////////////////////////////////////////
-// MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
-///////////////////////////////////////////////////////////////////////////////
-
-#define MPU9150_DEVICE_ADDR 0b01101000
-#define MPU9150_COMPASS_ADDR 0x0C
-
-#define ACCEL_GYRO_READ_SIZE 14 //Bytes
-#define ACCEL_GYRO_BASE_ADDR 0x3B //Starting register address
-
-#define MAG_READ_SIZE 6
-#define MAG_BASE_ADDR 0x03
-
-#define RAD_TO_DEG 57.29578
-#define DEG_TO_RAD 0.0174533
-
-// Array indicies when reading from ACCEL_GYRO_BASE_ADDR
-#define ACC_X_H 0
-#define ACC_X_L 1
-#define ACC_Y_H 2
-#define ACC_Y_L 3
-#define ACC_Z_H 4
-#define ACC_Z_L 5
-
-#define GYR_X_H 8
-#define GYR_X_L 9
-#define GYR_Y_H 10
-#define GYR_Y_L 11
-#define GYR_Z_H 12
-#define GYR_Z_L 13
-
-#define MAG_X_L 0
-#define MAG_X_H 1
-#define MAG_Y_L 2
-#define MAG_Y_H 3
-#define MAG_Z_L 4
-#define MAG_Z_H 5
-
-// Gyro is configured for +/-2000dps
-// Sensitivity gain is based off MPU9150 datasheet (pg. 11)
-#define GYRO_SENS 16.4
-
-#define GYRO_X_BIAS 0.005f
-#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.045f
-
-#define ACCEL_X_BIAS 0.023f
-#define ACCEL_Y_BIAS 0.009f
-#define ACCEL_Z_BIAS 0.087f
-
-void iic_set_globals(struct I2CDriver *given_i2c, struct SystemDriver *given_system);
-
-int iic0_mpu9150_write(u8 register_addr, u8 data);
-int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
-
-// Wake up the MPU for data collection
-// Configure Gyro/Accel/Mag
-int iic0_mpu9150_start();
-
-// Put MPU back to sleep
-void iic0_mpu9150_stop();
-
-void iic0_mpu9150_calc_mag_sensitivity();
-void iic0_mpu9150_read_mag(gam_t* gam);
-void iic0_mpu9150_read_gyro_accel(gam_t* gam);
-
-int iic_get_gam_timeout_failures();
-int iic_get_consecutive_gam_timeout_failures();
-int iic0_mpu9150_read_gam(gam_t* gam);
-
-////////////////////////////////////////////////////////////////////////////////////////////
-// LIDAR lite sensor defines (Addressing and registers specified on LIDAR lite v2 datasheet)
-////////////////////////////////////////////////////////////////////////////////////////////
-
-#define LIDARLITE_DEVICE_ADDR 0x62
-#define LIDAR_OFFSET 0.02 // Distance from LiDAR sensor to ground, in meters
-
-int iic0_lidarlite_write(u8 register_addr, u8 data);
-int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size);
-
-int iic0_lidarlite_read_distance(lidar_t *lidar);
-
-int iic0_lidarlite_init();
-int iic0_lidarlite_sleep();
-
-////////////////////////////////////////////////////////////////////////////////////////////
-// PX4FLOW optical flow sensor functions/defines (based on information on from pixhawk.org)
-////////////////////////////////////////////////////////////////////////////////////////////
-
-#define PX4FLOW_DEVICE_ADDR 0x42
-
-
-int iic0_px4flow_write(u8 register_addr, u8 data);
-int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size);
-
-int iic0_px4flow_update(px4flow_t *of);
-
-int iic0_px4flow_init(px4flow_t *of);
-
-
-#endif /*IIC_UTILS_H*/
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 5cd49342dcfffdaeb24649d18ca1555219200438..5d43e5a1df71ecaf041e1341d01b6c260509591f 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -9,32 +9,32 @@
#include "communication.h"
#include "controllers.h"
#include "sensor.h"
-#include "iic_utils.h"
//#define BENCH_TEST
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) {
@@ -74,7 +74,6 @@ int init_structs(modular_structs_t *structs) {
if (i2c->reset(i2c)) {
return -1;
}
- iic_set_globals(i2c, sys);
// Initialize PWM Recorders and Motor outputs
struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
@@ -94,8 +93,11 @@ int init_structs(modular_structs_t *structs) {
structs->user_defined_struct.flight_mode = MANUAL_FLIGHT_MODE;
// Get the first loop data from accelerometer for the gyroscope to use
- if(sensor_init(&structs->raw_sensor_struct, &structs->sensor_struct) == -1)
+ if(sensor_init(&structs->hardware_struct,
+ &structs->raw_sensor_struct,
+ &structs->sensor_struct)) {
return -1;
+ }
sensor_processing_init(&structs->sensor_struct);
diff --git a/quad/src/quad_app/initialize_components.h b/quad/src/quad_app/initialize_components.h
index e1737abd83be3fd793adf8e60ff8b7aa1f84b10e..a2a4ff721230176322d8844c089fc79039686eea 100644
--- a/quad/src/quad_app/initialize_components.h
+++ b/quad/src/quad_app/initialize_components.h
@@ -10,7 +10,6 @@
#include "timer.h"
#include "control_algorithm.h"
-#include "iic_utils.h"
#include "util.h"
#include "type_def.h"
#include "controllers.h"
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 3839b1b146e56e27fad559cd7d000c1e7ba8f6a3..1fd51a8c6ed1cf57822055659c123dc90df702b4 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -56,7 +56,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
get_user_input(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct));
// Get data from the sensors and put it into raw_sensor_struct
- get_sensors(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
+ get_sensors(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
// Process the sensor data and put it into sensor_struct
sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index c74e4a681d919472a8e72840027c6c9d74935cf7..7cd6a837542292274e4d3b54551ce00aa42cce75 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -10,82 +10,49 @@
#include "commands.h"
#include "type_def.h"
-int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
+int sensor_init(hardware_t *hardware_struct, raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
{
-// if (iic0_lidarlite_init()) { // init LiDAR
-// return -1;
-// }
-
- if(iic0_mpu9150_start()) {
- return -1;
- }
-
- if (iic0_px4flow_init(&raw_sensor_struct->optical_flow)) {
- return -1;
- }
-
- // read sensor board and fill in gryo/accelerometer/magnetometer struct
- iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
-
- // Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
- sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
- sensor_struct->roll_angle_filtered = raw_sensor_struct->gam.accel_pitch;
-
- return 0;
+ struct IMUDriver *imu = &hardware_struct->imu;
+ struct LidarDriver *lidar = &hardware_struct->lidar;
+ struct OpticalFlowDriver *of = &hardware_struct->of;
+
+ if (imu->reset(imu)) {
+ return -1;
+ }
+ if (lidar->reset(lidar)) {
+ return -1;
+ }
+ if (of->reset(of)) {
+ return -1;
+ }
+
+ imu->read(imu, &raw_sensor_struct->gam);
+
+ // Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
+ sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
+ sensor_struct->roll_angle_filtered = raw_sensor_struct->gam.accel_pitch;
+
+ return 0;
}
-int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
+int get_sensors(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
{
-// ///////// for testing update period of vrpn data from the ground station to the quad
-// static int update_counter = 0;
-// if(user_input_struct->hasPacket == 0x04)
-// {
-// char buf[200] = {};
-// // Send a reply to the ground station
-// snprintf(buf, sizeof(buf), "Received an update packet %dms\r\n", 5 * update_counter);
-// unsigned char *responsePacket;
-//
-// metadata_t metadata =
-// {
-// BEGIN_CHAR,
-// RESPONSE_TYPE_ID,
-// 0,
-// (strlen(buf) + 1)
-// };
-// formatPacket(&metadata, buf, MessageTypes[RESPONSE_TYPE_ID].functionPtr);
-//
-// // Send each byte of the packet individually
-// int i;
-// for(i = 0; i < 8 + metadata.data_len; i++) {
-// // Debug print statement for all of the bytes being sent
-//// printf("%d: 0x%x\n", i, responsePacket[i]);
-//
-// uart0_sendByte(responsePacket[i]);
-// }
-// update_counter = 0;
-// }
-// else
-// {
-// update_counter++;
-// }
-//
-// /////////// end testing
-
- int status = 0;
-
- // the the sensor board and fill in the readings into the GAM struct
- status |= iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
- log_struct->gam = raw_sensor_struct->gam;
-
- static lidar_t lidar_val;
- int lidar_status = iic0_lidarlite_read_distance(&lidar_val);
- if (lidar_status == 0) {
+ struct IMUDriver *imu = &hardware_struct->imu;
+ struct LidarDriver *lidar = &hardware_struct->lidar;
+ struct OpticalFlowDriver *of = &hardware_struct->of;
+ int status = 0;
+
+ status |= imu->read(imu, &raw_sensor_struct->gam);
+ static lidar_t lidar_val;
+ int lidar_status = lidar->read(lidar, &lidar_val);
+ if (lidar_status == 0) {
raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
- }
- status |= lidar_status;
-
- status |= iic0_px4flow_update(&raw_sensor_struct->optical_flow);
+ }
+ status |= lidar_status;
+
+ status |= of->read(of, &raw_sensor_struct->optical_flow);
+ log_struct->gam = raw_sensor_struct->gam;
- return status;
+ return 0;
}
diff --git a/quad/src/quad_app/sensor.h b/quad/src/quad_app/sensor.h
index 45791a5aa7830600b9750f73458bbd19fd066fd9..d8b9c2620408626afe187c000564968c006ac867 100644
--- a/quad/src/quad_app/sensor.h
+++ b/quad/src/quad_app/sensor.h
@@ -12,8 +12,8 @@
#include "log_data.h"
#include "user_input.h"
-#include "iic_utils.h"
#include "packet_processing.h"
+#include "hw_iface.h"
/**
* @brief
@@ -23,7 +23,7 @@
* error message
*
*/
-int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct);
+int sensor_init(hardware_t *hardware_struct, raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct);
/**
* @brief
@@ -42,6 +42,6 @@ int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct);
* error message
*
*/
-int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct);
+int get_sensors(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct);
#endif /* SENSOR_TEMPLATE_H_ */
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index e01e51dab61c1639dd06f7a7d23d6a7683561837..5f35184cc3f512fe6c6527ca8694c9530b4bdc0b 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -78,7 +78,7 @@ typedef struct {
//Gyro, accelerometer, and magnetometer data structure
//Used for reading an instance of the sensor data
-typedef struct {
+struct gam {
// GYRO
//Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
@@ -97,7 +97,6 @@ typedef struct {
float accel_roll;
float accel_pitch;
-
// MAG
//Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
@@ -106,24 +105,20 @@ typedef struct {
float mag_x; //Magnetic north: ~50 uT
float mag_y;
float mag_z;
+};
-
-
-}gam_t;
-
-typedef struct {
+struct lidar {
float distance_m; // distance in meters
-} lidar_t;
+};
-typedef struct {
+struct px4flow {
double xVel, yVel;
double distance;
double flowX, flowY;
- double gyroX, gyroY, gyroZ;
int16_t quality;
-} px4flow_t;
+};
typedef struct PID_t {
float current_point; // Current value of the system
@@ -450,6 +445,9 @@ typedef struct hardware_t {
struct TimerDriver axi_timer;
struct LEDDriver mio7_led;
struct SystemDriver sys;
+ struct IMUDriver imu;
+ struct LidarDriver lidar;
+ struct OpticalFlowDriver of;
} hardware_t;
/**
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/.gitignore b/quad/src/virt_quad/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..a72889f170892acba1014b48362cee10940f1fc4
--- /dev/null
+++ b/quad/src/virt_quad/.gitignore
@@ -0,0 +1,2 @@
+virt-quad
+virt-quad-fifos/
\ No newline at end of file
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.c b/quad/src/virt_quad/hw_impl_unix.c
index bbd8e8775df179f41c36e3f96152f634bf1e0d97..d3607f17e16deffee36a4cefe64a6b0a55f15943 100644
--- a/quad/src/virt_quad/hw_impl_unix.c
+++ b/quad/src/virt_quad/hw_impl_unix.c
@@ -68,3 +68,27 @@ struct SystemDriver create_unix_system() {
sys.sleep = unix_system_sleep;
return sys;
}
+
+struct IMUDriver create_unix_imu(struct I2CDriver *i2c) {
+ struct IMUDriver imu;
+ imu.i2c = i2c;
+ imu.reset = unix_imu_reset;
+ imu.read = unix_imu_read;
+ return imu;
+}
+
+struct LidarDriver create_unix_lidar(struct I2CDriver *i2c) {
+ struct LidarDriver lidar;
+ lidar.i2c = i2c;
+ lidar.reset = unix_lidar_reset;
+ lidar.read = unix_lidar_read;
+ return lidar;
+}
+
+struct OpticalFlowDriver create_unix_optical_flow(struct I2CDriver *i2c) {
+ struct OpticalFlowDriver of;
+ of.i2c = i2c;
+ of.reset = unix_optical_flow_reset;
+ of.read = unix_optical_flow_read;
+ return of;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index 85e223f2896a317fd5f1c1fa79f6fc60cb7a8772..18119fb6b8ec03857a38e85e37fa8e3cdb409f99 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -9,9 +9,30 @@
#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];
+ pthread_mutex_t i2c_lock;
+ gam_t gam;
+ lidar_t lidar;
+ px4flow_t of;
+};
+
+#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);
@@ -47,6 +68,15 @@ int unix_mio7_led_turn_off(struct LEDDriver *self);
int unix_system_reset(struct SystemDriver *self);
int unix_system_sleep(struct SystemDriver *self, unsigned long us);
+int unix_imu_reset(struct IMUDriver *self);
+int unix_imu_read(struct IMUDriver *self, gam_t *gam);
+
+int unix_lidar_reset(struct LidarDriver *self);
+int unix_lidar_read(struct LidarDriver *self, lidar_t *lidar);
+
+int unix_optical_flow_reset(struct OpticalFlowDriver *self);
+int unix_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of);
+
struct UARTDriver create_unix_uart();
struct MotorDriver create_unix_motors();
struct RCReceiverDriver create_unix_rc_receiver();
@@ -55,6 +85,9 @@ struct TimerDriver create_unix_global_timer();
struct TimerDriver create_unix_axi_timer();
struct LEDDriver create_unix_mio7_led();
struct SystemDriver create_unix_system();
+struct IMUDriver create_unix_imu(struct I2CDriver *i2c);
+struct LidarDriver create_unix_lidar(struct I2CDriver *i2c);
+struct OpticalFlowDriver create_unix_optical_flow(struct I2CDriver *i2c);
int test_unix_i2c();
int test_unix_mio7_led_and_system();
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index 97228f0bada9ca366f30c145f66c54edae0cedde..7bdd2d39be954f46505d6e439fac907bc514897c 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -3,53 +3,8 @@
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>
-#include "iic_utils.h"
-
-#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 +12,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_imu.c b/quad/src/virt_quad/hw_impl_unix_imu.c
new file mode 100644
index 0000000000000000000000000000000000000000..33639b424c349e0e0bc293fc924cdac7b0b519f9
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_imu.c
@@ -0,0 +1,25 @@
+#include "hw_iface.h"
+#include "hw_impl_unix.h"
+
+extern struct VirtQuadIO *virt_quad_io;
+
+int unix_imu_reset(struct IMUDriver *self) {
+ // Sensible defaults
+ virt_quad_io->gam.gyro_xVel_p = 0;
+ virt_quad_io->gam.gyro_yVel_q = 0;
+ virt_quad_io->gam.gyro_zVel_r = 0;
+ virt_quad_io->gam.accel_x = 0;
+ virt_quad_io->gam.accel_y = 0;
+ virt_quad_io->gam.accel_z = -1;
+ virt_quad_io->gam.mag_x = 0;
+ virt_quad_io->gam.mag_y = 0;
+ virt_quad_io->gam.mag_z = 0;
+ virt_quad_io->gam.accel_roll = 0;
+ virt_quad_io->gam.accel_pitch = 0;
+ return 0;
+}
+
+int unix_imu_read(struct IMUDriver *self, gam_t *gam) {
+ *gam = virt_quad_io->gam;
+ return 0;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_lidar.c b/quad/src/virt_quad/hw_impl_unix_lidar.c
new file mode 100644
index 0000000000000000000000000000000000000000..fdf18894db7d6f35bc741e64e1a99cee079ff900
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_lidar.c
@@ -0,0 +1,14 @@
+#include "hw_iface.h"
+#include "hw_impl_unix.h"
+
+extern struct VirtQuadIO *virt_quad_io;
+
+int unix_lidar_reset(struct LidarDriver *self) {
+ virt_quad_io->lidar.distance_m = 0;
+ return 0;
+}
+
+int unix_lidar_read(struct LidarDriver *self, lidar_t *lidar) {
+ *lidar = virt_quad_io->lidar;
+ return 0;
+}
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 0a7d752fbb70a214fb5c934771244f9b54b602c8..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();
-
-volatile 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_optical_flow.c b/quad/src/virt_quad/hw_impl_unix_optical_flow.c
new file mode 100644
index 0000000000000000000000000000000000000000..b2cf59f075104947513f85b7689ade116b65eb92
--- /dev/null
+++ b/quad/src/virt_quad/hw_impl_unix_optical_flow.c
@@ -0,0 +1,15 @@
+#include "hw_iface.h"
+#include "hw_impl_unix.h"
+
+extern struct VirtQuadIO *virt_quad_io;
+
+int unix_optical_flow_reset(struct OpticalFlowDriver *self) {
+ virt_quad_io->of.xVel = 0;
+ virt_quad_io->of.yVel = 0;
+ return 0;
+}
+
+int unix_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
+ *of = virt_quad_io->of;
+ return 0;
+}
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..b4315760fe1a0e28b0ee44869169f89a029e56af 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,25 @@
#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]);
- }
-
+ // sane defaults for the RC receiver
+ virt_quad_io->rc_receiver[0] = 0;
+ virt_quad_io->rc_receiver[1] = 0.5;
+ virt_quad_io->rc_receiver[2] = 0.5;
+ virt_quad_io->rc_receiver[3] = 0.5;
+ virt_quad_io->rc_receiver[4] = 0;
+ virt_quad_io->rc_receiver[5] = 0;
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..105a8a5e68ae04d21e8001b9eb8c0b78a8629038 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -4,6 +4,19 @@
#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);
+void print_shm_float(float *value, pthread_mutex_t *lock);
+void print_shm_int(int *value, pthread_mutex_t *lock);
+void usage(char *executable_name);
+
+struct VirtQuadIO *virt_quad_io;
+int virt_quad_io_file_descriptor;
+
+/**
+ * Implement each of the hardware interfaces.
+ */
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_unix_i2c();
hardware->rc_receiver = create_unix_rc_receiver();
@@ -13,24 +26,202 @@ int setup_hardware(hardware_t *hardware) {
hardware->axi_timer = create_unix_axi_timer();
hardware->mio7_led = create_unix_mio7_led();
hardware->sys = create_unix_system();
+ hardware->imu = create_unix_imu(&hardware->i2c);
+ hardware->lidar = create_unix_lidar(&hardware->i2c);
+ hardware->of = create_unix_optical_flow(&hardware->i2c);
return 0;
}
int main(int argc, char *argv[]) {
- int fd, opt;
- while ((opt = getopt(argc, argv, "q")) != -1) {
- switch (opt) {
- case 'q':
+ int fd;
+
+ // Decide if we are launching the quad or parsing arguments
+ if (argv[1] == NULL || strcmp(argv[1], "-q") == 0) {
+ // launch the quad
+
+ // Allow making the quad quiet
+ if (argv[1] != NULL && strcmp(argv[1], "-q") == 0) {
fd = open("/dev/null", O_WRONLY);
close(STDOUT_FILENO);
dup2(STDOUT_FILENO, fd);
- break;
- default: /* '?' */
- fprintf(stderr, "Usage: %s [-q]\n", argv[0]);
- exit(EXIT_FAILURE);
}
+
+ // Prepare the shared memory for io. Clear memory and initialize.
+ 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);
+ }
+ else {
+ // parse command line arguments
+
+ // 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);
+
+ if (strcmp(argv[1], "--help") == 0 || strcmp(argv[1], "-h") == 0) {
+ usage(argv[0]);
+ exit(0);
+ }
+ else if (strcmp(argv[1], "get") == 0 && argv[2] != NULL) {
+ handle_io_output(argv[2]);
+ }
+ else if (strcmp(argv[1], "set") == 0 && argv[2] != NULL && argv[3] != NULL) {
+ handle_io_input(argv[2], argv[3]);
+ }
+ else {
+ puts("Error in parsing commands");
+ usage(argv[0]);
+ }
+ 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) {
+ print_shm_int(&io->led, &io->led_lock);
+ }
+ else if (strcmp(name, "motor1") == 0) {
+ print_shm_float(&io->motors[0], &io->motors_lock);
+ }
+ else if (strcmp(name, "motor2") == 0) {
+ print_shm_float(&io->motors[1], &io->motors_lock);
+ }
+ else if (strcmp(name, "motor3") == 0) {
+ print_shm_float(&io->motors[2], &io->motors_lock);
+ }
+ else if (strcmp(name, "motor4") == 0) {
+ print_shm_float(&io->motors[3], &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 if (strcmp(name, "i2c_imu_x") == 0) {
+ set_shm(value, &io->gam.accel_x, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_y") == 0) {
+ set_shm(value, &io->gam.accel_y, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_z") == 0) {
+ set_shm(value, &io->gam.accel_z, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_p") == 0) {
+ set_shm(value, &io->gam.gyro_xVel_p, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_q") == 0) {
+ set_shm(value, &io->gam.gyro_yVel_q, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_r") == 0) {
+ set_shm(value, &io->gam.gyro_zVel_r, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_mag_x") == 0) {
+ set_shm(value, &io->gam.mag_x, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_mag_y") == 0) {
+ set_shm(value, &io->gam.mag_y, &io->i2c_lock);
+ }
+ else if (strcmp(name, "i2c_imu_mag_z") == 0) {
+ set_shm(value, &io->gam.mag_z, &io->i2c_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);
+}
+
+void print_shm_float(float *value, pthread_mutex_t *lock) {
+ pthread_mutex_lock(lock);
+ printf("%f\n", *value);
+ pthread_mutex_unlock(lock);
+}
+
+void print_shm_int(int *value, pthread_mutex_t *lock) {
+ pthread_mutex_lock(lock);
+ printf("%d\n", *value);
+ pthread_mutex_unlock(lock);
+}
+
+void usage(char *executable_name) {
+ printf("Usage: %s [ -h | --help | -q ] [ command ]\n", executable_name);
+ puts("Overview:");
+ puts(" The virtual quad emulates the behavior of the real quad in the Unix");
+ puts(" environment. Start the virtual quad in one tab (no arguments), and");
+ puts(" then control the I/O of the quad through commands.");
+ puts("Commands:");
+ puts(" get output");
+ puts(" set input value");
+ puts("Outpus:");
+ puts(" led");
+ puts(" motors");
+ puts("Inputs:");
+ puts(" rc_gear");
+ puts(" rc_flap");
+ puts(" rc_throttle");
+ puts(" rc_pitch");
+ puts(" rc_roll");
+ puts(" rc_yaw");
+ puts("Examples:");
+ printf(" %s get led # prints 0 or 1 to stdout\n", executable_name);
+ printf(" in%s set rc_gear 1 # sets gear to \"on\" \n", executable_name);
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
index 3a6348f9b9a8f8090b2769f17201aeda88f36b93..cd3f622668a0ff24857ba7324f361a8025638975 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
@@ -68,3 +68,27 @@ struct SystemDriver create_zybo_system() {
sys.sleep = zybo_system_sleep;
return sys;
}
+
+struct IMUDriver create_zybo_imu(struct I2CDriver *i2c) {
+ struct IMUDriver imu;
+ imu.i2c = i2c;
+ imu.reset = zybo_imu_reset;
+ imu.read = zybo_imu_read;
+ return imu;
+}
+
+struct LidarDriver create_zybo_lidar(struct I2CDriver *i2c) {
+ struct LidarDriver lidar;
+ lidar.i2c = i2c;
+ lidar.reset = zybo_lidar_reset;
+ lidar.read = zybo_lidar_read;
+ return lidar;
+}
+
+struct OpticalFlowDriver create_zybo_optical_flow(struct I2CDriver *i2c) {
+ struct OpticalFlowDriver of;
+ of.i2c = i2c;
+ of.reset = zybo_optical_flow_reset;
+ of.read = zybo_optical_flow_read;
+ return of;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
index 29d0ff8f388e0780f59aeefa63515ca0e8031522..fe2c11c67e5ca28bb601e9f04fa7b4f37b7251ce 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
@@ -18,6 +18,11 @@
#include "platform.h"
#include "sleep.h"
+// Ideally, these defines would only be in the optical flow file, but
+// i2c needs it for a certain hack
+#define PX4FLOW_DEVICE_ADDR 0x42
+#define PX4FLOW_QUAL_MIN (100)
+
int zybo_uart_reset(struct UARTDriver *self);
int zybo_uart_write(struct UARTDriver *self, unsigned char c);
int zybo_uart_read(struct UARTDriver *self, unsigned char *c);
@@ -53,6 +58,15 @@ int zybo_mio7_led_turn_off(struct LEDDriver *self);
int zybo_system_reset(struct SystemDriver *self);
int zybo_system_sleep(struct SystemDriver *self, unsigned long us);
+int zybo_imu_reset(struct IMUDriver *self);
+int zybo_imu_read(struct IMUDriver *self, gam_t *gam);
+
+int zybo_lidar_reset(struct LidarDriver *self);
+int zybo_lidar_read(struct LidarDriver *self, lidar_t *lidar);
+
+int zybo_optical_flow_reset(struct OpticalFlowDriver *self);
+int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of);
+
struct UARTDriver create_zybo_uart();
struct MotorDriver create_zybo_motors();
struct RCReceiverDriver create_zybo_rc_receiver();
@@ -61,6 +75,9 @@ struct TimerDriver create_zybo_global_timer();
struct TimerDriver create_zybo_axi_timer();
struct LEDDriver create_zybo_mio7_led();
struct SystemDriver create_zybo_system();
+struct IMUDriver create_zybo_imu(struct I2CDriver *i2c);
+struct LidarDriver create_zybo_lidar(struct I2CDriver *i2c);
+struct OpticalFlowDriver create_zybo_optical_flow(struct I2CDriver *i2c);
int test_zybo_i2c();
int test_zybo_mio7_led_and_system();
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
index 203fd3587ed1c74620e44f2b27b06044a392c56f..e2e3568e0b581caed47c00accac275d552433fa6 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
@@ -1,8 +1,39 @@
#include "hw_impl_zybo.h"
-#include "iic_utils.h"
#include "xiicps.h"
#include "timer.h"
+// System configuration registers
+// (Please see Appendix B: System Level Control Registers in the Zybo TRM)
+#define IIC_SYSTEM_CONTROLLER_RESET_REG_ADDR (0xF8000224)
+#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
+
+// Error code could be returned on an iic read if the watchdog timer triggers
+#define IIC_RX_TIMEOUT_FAILURE (-88)
+
+// IIC0 Registers
+#define IIC0_CONTROL_REG_ADDR (0xE0004000)
+#define IIC0_STATUS_REG_ADDR (0xE0004004)
+#define IIC0_SLAVE_ADDR_REG (0xE0004008)
+#define IIC0_DATA_REG_ADDR (0xE000400C)
+#define IIC0_INTR_STATUS_REG_ADDR (0xE0004010)
+#define IIC0_TRANFER_SIZE_REG_ADDR (0xE0004014)
+#define IIC0_INTR_EN (0xE0004024)
+#define IIC0_TIMEOUT_REG_ADDR (0xE000401C)
+
+//Interrupt Status Register Masks
+#define ARB_LOST (0x200)
+#define RX_UNF (0x80)
+#define TX_OVF (0x40)
+#define RX_OVF (0x20)
+#define SLV_RDY (0x10)
+#define TIME_OUT (0x08)
+#define NACK (0x04)
+#define MORE_DAT (0x02)
+#define TRANS_COMPLETE (0x01)
+
+#define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
+#define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
+
#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
@@ -17,7 +48,7 @@ int zybo_i2c_reset(struct I2CDriver *self) {
}
XIicPs *inst = self->state;
- //Make sure CPU_1x clk is enabled for I2C controller
+ //Make sure CPU_1x clk is enabled fostatusr I2C controller
u16 *aper_ctrl = (u16 *) IO_CLK_CONTROL_REG_ADDR;
if (*aper_ctrl & 0x00040000){
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
new file mode 100644
index 0000000000000000000000000000000000000000..d215cea0617e77e07116dab9b84afba0cd91655f
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -0,0 +1,225 @@
+#include "hw_iface.h"
+#include "hw_impl_zybo.h"
+#include "type_def.h"
+
+#define MPU9150_DEVICE_ADDR 0b01101000
+#define MPU9150_COMPASS_ADDR 0x0C
+
+#define ACCEL_GYRO_READ_SIZE 14 //Bytes
+#define ACCEL_GYRO_BASE_ADDR 0x3B //Starting register address
+
+#define MAG_READ_SIZE 6
+#define MAG_BASE_ADDR 0x03
+
+#define RAD_TO_DEG 57.29578
+#define DEG_TO_RAD 0.0174533
+
+// Array indicies when reading from ACCEL_GYRO_BASE_ADDR
+#define ACC_X_H 0
+#define ACC_X_L 1
+#define ACC_Y_H 2
+#define ACC_Y_L 3
+#define ACC_Z_H 4
+#define ACC_Z_L 5
+
+#define GYR_X_H 8
+#define GYR_X_L 9
+#define GYR_Y_H 10
+#define GYR_Y_L 11
+#define GYR_Z_H 12
+#define GYR_Z_L 13
+
+#define MAG_X_L 0
+#define MAG_X_H 1
+#define MAG_Y_L 2
+#define MAG_Y_H 3
+#define MAG_Z_L 4
+#define MAG_Z_H 5
+
+// Gyro is configured for +/-2000dps
+// Sensitivity gain is based off MPU9150 datasheet (pg. 11)
+#define GYRO_SENS 16.4
+
+#define GYRO_X_BIAS 0.005f
+#define GYRO_Y_BIAS -0.014f
+#define GYRO_Z_BIAS 0.045f
+
+#define ACCEL_X_BIAS 0.023f
+#define ACCEL_Y_BIAS 0.009f
+#define ACCEL_Z_BIAS 0.087f
+
+int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
+int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
+
+int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
+int mpu9150_read_gyro_accel(gam_t* gam);
+
+int zybo_imu_reset(struct IMUDriver *self) {
+ struct I2CDriver *i2c = self->i2c;
+
+ // Device Reset & Wake up
+ mpu9150_write(i2c, 0x6B, 0x80);
+ usleep(5000);
+
+ // Set clock reference to Z Gyro
+ mpu9150_write(i2c, 0x6B, 0x03);
+ // Configure Digital Low/High Pass filter
+ mpu9150_write(i2c, 0x1A,0x03); // Level 3 low pass on gyroscope
+
+ // Configure Gyro to 2000dps, Accel. to +/-8G
+ mpu9150_write(i2c, 0x1B, 0x18);
+ mpu9150_write(i2c, 0x1C, 0x10);
+
+ // Enable I2C bypass for AUX I2C (Magnetometer)
+ mpu9150_write(i2c, 0x37, 0x02);
+
+ // Setup Mag
+ mpu9150_write(i2c, 0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
+
+ usleep(100000);
+
+ int i;
+ gam_t temp_gam;
+
+ // Do about 20 reads to warm up the device
+ for(i=0; i < 20; ++i){
+ self->read(self, &temp_gam);
+ usleep(1000);
+ }
+
+ return 0;
+}
+
+int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
+ struct I2CDriver *i2c = self->i2c;
+ i16 raw_accel_x, raw_accel_y, raw_accel_z;
+ i16 gyro_x, gyro_y, gyro_z;
+
+ u8 sensor_data[ACCEL_GYRO_READ_SIZE] = {};
+
+ int error = mpu9150_read(i2c, sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+ if (error) return error;
+
+ //Calculate accelerometer data
+ raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
+ raw_accel_y = sensor_data[ACC_Y_H] << 8 | sensor_data[ACC_Y_L];
+ raw_accel_z = sensor_data[ACC_Z_H] << 8 | sensor_data[ACC_Z_L];
+
+ // put in G's
+ gam->accel_x = (raw_accel_x / 4096.0) + ACCEL_X_BIAS; // 4,096 is the gain per LSB of the measurement reading based on a configuration range of +-8g
+ gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
+ gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
+
+ //Convert gyro data to rate (we're only using the most 12 significant bits)
+ gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
+ gyro_y = (sensor_data[GYR_Y_H] << 8 | sensor_data[GYR_Y_L]);// * G_GAIN;
+ gyro_z = (sensor_data[GYR_Z_H] << 8 | sensor_data[GYR_Z_L]);// * G_GAIN;
+
+ //Get the number of degrees
+ //javey: converted to radians to following SI units
+ gam->gyro_xVel_p = ((gyro_x / GYRO_SENS) * DEG_TO_RAD) + GYRO_X_BIAS;
+ gam->gyro_yVel_q = ((gyro_y / GYRO_SENS) * DEG_TO_RAD) + GYRO_Y_BIAS;
+ gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
+
+ // Magnometer
+ //mpu9150_read_mag(self, gam);
+
+ return error;
+}
+
+//////////////////////
+// Helper functions
+/////////////////////
+
+int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data){
+
+ u16 device_addr = MPU9150_DEVICE_ADDR;
+ u8 buf[] = {register_addr, data};
+
+ // Check if within register range
+ if(register_addr < 0 || register_addr > 0x75){
+ return;
+ }
+
+ if(register_addr <= 0x12){
+ device_addr = MPU9150_COMPASS_ADDR;
+ }
+
+ return i2c->write(i2c, device_addr, buf, 2);
+}
+
+int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size){
+
+ u16 device_addr = MPU9150_DEVICE_ADDR;
+ u8 buf[] = {register_addr};
+
+ // Check if within register range
+ if(register_addr < 0 || register_addr > 0x75){
+ }
+
+ // Set device address to the if 0x00 <= register address <= 0x12
+ if(register_addr <= 0x12){
+ device_addr = MPU9150_COMPASS_ADDR;
+ }
+
+
+ int error = i2c->write(i2c, device_addr, buf, 1);
+ if (error) return error;
+ return i2c->read(i2c, device_addr, recv_buffer, size);
+}
+
+
+int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam){
+
+ static double magX_correction = -1, magY_correction, magZ_correction;
+
+ struct I2CDriver *i2c = self->i2c;
+
+ u8 mag_data[6];
+ i16 raw_magX, raw_magY, raw_magZ;
+
+ // Grab calibrations if not done already
+ if(magX_correction == -1){
+ u8 buf[3];
+ u8 ASAX, ASAY, ASAZ;
+
+ // Quickly read from the factory ROM to get correction coefficents
+ mpu9150_write(i2c, 0x0A, 0x0F);
+ usleep(10000);
+
+ // Read raw adjustment values
+ mpu9150_read(i2c, buf, 0x10,3);
+ ASAX = buf[0];
+ ASAY = buf[1];
+ ASAZ = buf[2];
+
+ // Set the correction coefficients
+ magX_correction = (ASAX-128)*0.5/128 + 1;
+ magY_correction = (ASAY-128)*0.5/128 + 1;
+ magZ_correction = (ASAZ-128)*0.5/128 + 1;
+ }
+
+ // Set Mag to single read mode
+ mpu9150_write(i2c, 0x0A, 0x01);
+ usleep(10000);
+ mag_data[0] = 0;
+
+ // Keep checking if data is ready before reading new mag data
+ while(mag_data[0] == 0x00){
+ mpu9150_read(i2c, mag_data, 0x02, 1);
+ }
+
+ // Get mag data
+ mpu9150_read(i2c, mag_data, 0x03, 6);
+
+ raw_magX = (mag_data[1] << 8) | mag_data[0];
+ raw_magY = (mag_data[3] << 8) | mag_data[2];
+ raw_magZ = (mag_data[5] << 8) | mag_data[4];
+
+ // Set magnetometer data to output
+ gam->mag_x = raw_magX * magX_correction;
+ gam->mag_y = raw_magY * magY_correction;
+ gam->mag_z = raw_magZ * magZ_correction;
+
+ return 0;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
new file mode 100644
index 0000000000000000000000000000000000000000..f03ec059b4e90694e1218101158fb13325f2df11
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
@@ -0,0 +1,64 @@
+#include "hw_iface.h"
+#include "hw_impl_zybo.h"
+#include "type_def.h"
+
+#define LIDARLITE_DEVICE_ADDR 0x62
+#define LIDAR_OFFSET 0.02 // Distance from LiDAR sensor to ground, in meters
+
+int lidarlite_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
+int lidarlite_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
+
+int zybo_lidar_reset(struct LidarDriver *self) {
+ struct I2CDriver *i2c = self->i2c;
+
+ int error = 0;
+
+ // Device Reset & Wake up with default settings
+ error = lidarlite_write(i2c, 0x00, 0x00);
+ if (error) return error;
+ usleep(15000);
+
+ // Enable Free Running Mode and distance measurements with correction
+ error = lidarlite_write(i2c, 0x11, 0xff);
+ if (error) return error;
+ error = lidarlite_write(i2c, 0x00, 0x04);
+ return error;
+}
+
+int zybo_lidar_read(struct LidarDriver *self, lidar_t *lidar) {
+ struct I2CDriver *i2c = self->i2c;
+ u8 buf[2];
+ int error = 0;
+
+ // Read the sensor value
+ error = lidarlite_read(i2c, buf, 0x8f, 2);
+ if (error) return error;
+ float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
+ lidar->distance_m = distance_cm * 0.01;
+
+ return error;
+}
+
+////////////////////
+// Helper functions
+////////////////////
+
+int lidarlite_write(struct I2CDriver *i2c, u8 register_addr, u8 data) {
+ u8 buf[] = {register_addr, data};
+ return i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 2);
+}
+
+int lidarlite_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size) {
+ u8 buf[] = {register_addr};
+ int error = 0;
+
+ error = i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
+ if (error) return error;
+ error = i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
+ return error;
+}
+
+// Maybe this will be useful?
+int lidarlite_sleep(struct I2CDriver *i2c) {
+ return lidarlite_write(i2c, 0x65, 0x84);
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
new file mode 100644
index 0000000000000000000000000000000000000000..f6d463987ffb38160d48abf2b2181f414aac3fd3
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
@@ -0,0 +1,73 @@
+#include "hw_iface.h"
+#include "hw_impl_zybo.h"
+#include <stdint.h>
+#include "type_def.h"
+
+int px4flow_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
+int px4flow_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
+
+int zybo_optical_flow_reset(struct OpticalFlowDriver *self) {
+ // Nothing to do, device is plug-and-play
+ return 0;
+}
+
+int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
+ struct I2CDriver *i2c = self->i2c;
+ int error = 0;
+
+ struct {
+ uint16_t frameCount;
+
+ int16_t pixelFlowXSum;
+ int16_t pixelFlowYSum;
+ int16_t flowCompX;
+ int16_t flowCompY;
+ int16_t qual;
+
+ int16_t gyroXRate;
+ int16_t gyroYRate;
+ int16_t gyroZRate;
+
+ uint8_t gyroRange;
+ uint8_t sonarTimestamp;
+ int16_t groundDistance;
+ } i2cFrame;
+
+ u8 buf[sizeof(i2cFrame)];
+
+ // Read the sensor value
+ error = px4flow_read(i2c, buf, 0x00, sizeof(i2cFrame));
+
+ if(error == 0) {
+ //Copy into struct
+ memcpy(&i2cFrame, buf, sizeof(i2cFrame));
+
+ of->xVel = i2cFrame.flowCompX / 1000.;
+ of->yVel = i2cFrame.flowCompY / 1000.;
+ of->quality = i2cFrame.qual;
+ of->distance = i2cFrame.groundDistance / 1000.;
+ }
+
+ return error;
+}
+
+/////////////////////
+// Helper functions
+/////////////////////
+
+int px4flow_write(struct I2CDriver *i2c, u8 register_addr, u8 data) {
+ u8 buf[] = {register_addr, data};
+
+ return i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 2);
+}
+
+int px4flow_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size) {
+ u8 buf[] = {register_addr};
+ int error = 0;
+
+ error = i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
+ if (error) return error;
+ error = i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+ return error;
+}
+
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index cf248f6af93600632918105cf6dc897b2d0362e1..1e74131ccf55d582139f9acbeb876fb217fdffe9 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -1,6 +1,5 @@
#include "hw_impl_zybo.h"
#include "type_def.h"
-#include "iic_utils.h"
#include <stdio.h>
/**
@@ -52,72 +51,60 @@ int test_zybo_mio7_led_and_system() {
*/
int test_zybo_i2c() {
struct I2CDriver i2c = create_zybo_i2c();
- struct SystemDriver sys = create_zybo_system();
+ struct LidarDriver ld = create_zybo_lidar(&i2c);
i2c.reset(&i2c);
- sys.reset(&sys);
lidar_t lidar = { };
- iic_set_globals(&i2c, &sys);
- if (iic0_lidarlite_init()) return 0;
- float x;
+ if (ld.reset(&ld)) return 0;
while (1) {
- iic0_lidarlite_read_distance(&lidar);
- x = lidar.distance_m;
+ ld.read(&ld, &lidar);
}
return 0;
}
int test_zybo_i2c_imu() {
struct I2CDriver i2c = create_zybo_i2c();
- struct SystemDriver sys = create_zybo_system();
+ struct IMUDriver imu = create_zybo_imu(&i2c);
i2c.reset(&i2c);
- sys.reset(&sys);
+ if (imu.reset(&imu)) return 0;
gam_t gam = { };
- iic_set_globals(&i2c, &sys);
- if (iic0_mpu9150_start()) return 0;
- short x;
while (1) {
- iic0_mpu9150_read_gam(&gam);
+ imu.read(&imu, &gam);
}
return 0;
}
int test_zybo_i2c_px4flow() {
struct I2CDriver i2c = create_zybo_i2c();
- struct SystemDriver sys = create_zybo_system();
+ struct OpticalFlowDriver ofd = create_zybo_optical_flow(&i2c);
i2c.reset(&i2c);
- sys.reset(&sys);
- px4flow_t of;
- iic_set_globals(&i2c, &sys);
+ if (ofd.reset(&ofd)) return 0;
- if(iic0_px4flow_init(&of)) return 0;
+ px4flow_t of;
for(;;) {
- unsigned int delay = 5;
-
- sys.sleep(&sys, delay * 1000);
-
- iic0_px4flow_update(&of);
- }
+ usleep(5000);
+ ofd.read(&ofd, &of);
+ } struct IMUDriver imu = create_zybo_imu(&i2c);
return 0;
}
int test_zybo_i2c_all() {
struct I2CDriver i2c = create_zybo_i2c();
- struct SystemDriver sys = create_zybo_system();
+ struct IMUDriver imu = create_zybo_imu(&i2c);
+ struct LidarDriver ld = create_zybo_lidar(&i2c);
+ struct OpticalFlowDriver ofd = create_zybo_optical_flow(&i2c);
i2c.reset(&i2c);
- sys.reset(&sys);
+
+ if (ld.reset(&ld)) return 0;
+ if (imu.reset(&imu)) return 0;
+ if (ofd.reset(&ofd)) return 0;
lidar_t lidar = { };
px4flow_t of = { };
gam_t gam = { };
- iic_set_globals(&i2c, &sys);
-
- if (iic0_px4flow_init(&of)) return 0;
- if (iic0_mpu9150_start()) return 0;
- if (iic0_lidarlite_init()) return 0;
int lidarErrors = 0;
int gamErrors = 0;
@@ -125,16 +112,9 @@ int test_zybo_i2c_all() {
int of_errors = 0;
for(;;) {
- unsigned int delay = 5;
-
- sys.sleep(&sys, delay * 1000);
-
- if (iic0_px4flow_update(&of)) {
- of_errors += 1;
- }
-
- iic0_mpu9150_read_gam(&gam);
- iic0_lidarlite_read_distance(&lidar);
+ ld.read(&ld, &lidar);
+ imu.read(&imu, &gam);
+ ofd.read(&ofd, &of);
if (lidar.distance_m > 50) {
lidarErrors += 1;
@@ -191,8 +171,6 @@ int test_zybo_rc_receiver() {
int test_zybo_motors() {
struct MotorDriver motors = create_zybo_motors();
motors.reset(&motors);
- struct SystemDriver sys = create_zybo_system();
- sys.reset(&sys);
double j = 0;
while (1) {
@@ -200,7 +178,7 @@ int test_zybo_motors() {
int i = 0;
for (i = 0; i < 4; i += 1) {
motors.write(&motors, i, j);
- sys.sleep(&sys, 50000);
+ usleep(50000);
}
}
}
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 6eeb6ce03ce9615daaee02626ea1fdbab0a216d1..8ab3861beb7dd193fdc44845092854e3f19de0d8 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -7,7 +7,6 @@
//#define RUN_TESTS
int setup_hardware(hardware_t *hardware) {
- hardware->i2c = create_zybo_i2c();
hardware->rc_receiver = create_zybo_rc_receiver();
hardware->motors = create_zybo_motors();
hardware->uart = create_zybo_uart();
@@ -15,6 +14,10 @@ int setup_hardware(hardware_t *hardware) {
hardware->axi_timer = create_zybo_axi_timer();
hardware->mio7_led = create_zybo_mio7_led();
hardware->sys = create_zybo_system();
+ hardware->i2c = create_zybo_i2c();
+ hardware->imu = create_zybo_imu(&hardware->i2c);
+ hardware->lidar = create_zybo_lidar(&hardware->i2c);
+ hardware->of = create_zybo_optical_flow(&hardware->i2c);
return 0;
}
@@ -26,11 +29,11 @@ int main()
#ifdef RUN_TESTS
//test_zybo_mio7_led_and_system();
//test_zybo_i2c();
- //test_zybo_i2c_imu();
+ test_zybo_i2c_imu();
//test_zybo_i2c_px4flow();
//test_zybo_i2c_all();
//test_zybo_rc_receiver();
- test_zybo_motors();
+ //test_zybo_motors();
//test_zybo_uart();
//test_zybo_axi_timer();
//test_zybo_uart();