From 287a9c2a0f924692b7c71c4ed10c48060b2f6090 Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Wed, 12 Apr 2017 07:33:51 -0500
Subject: [PATCH] quad: use normalized pwm value throughout app
---
quad/scripts/tests/test_safety_checks.rb | 28 ++--
quad/scripts/tests/testing_library.rb | 38 ++---
quad/src/graph_blocks/node_mixer.c | 28 ++--
quad/src/graph_blocks/node_mixer.h | 8 +-
quad/src/quad_app/PID.h | 16 +-
.../quad_app/actuator_command_processing.c | 143 ------------------
.../quad_app/actuator_command_processing.h | 38 -----
quad/src/quad_app/control_algorithm.c | 18 +--
quad/src/quad_app/controllers.h | 42 ++---
quad/src/quad_app/hw_iface.h | 12 +-
quad/src/quad_app/initialize_components.c | 22 +--
quad/src/quad_app/log_data.c | 8 +-
quad/src/quad_app/quad_app.c | 7 +-
quad/src/quad_app/send_actuator_commands.c | 4 +-
quad/src/quad_app/send_actuator_commands.h | 3 +-
quad/src/quad_app/sensor.c | 12 +-
quad/src/quad_app/sensor_processing.h | 39 -----
quad/src/quad_app/type_def.h | 10 +-
quad/src/quad_app/user_input.c | 8 +-
quad/src/quad_app/user_input.h | 32 +---
quad/src/quad_app/util.c | 37 +++--
quad/src/quad_app/util.h | 8 +-
quad/src/virt_quad/hw_impl_unix.c | 24 +--
quad/src/virt_quad/hw_impl_unix.h | 14 +-
...unix_pwm_output.c => hw_impl_unix_motor.c} | 29 ++--
...pwm_input.c => hw_impl_unix_rc_receiver.c} | 32 ++--
quad/src/virt_quad/main.c | 4 +-
.../real_quad/src/hw_impl_zybo.c | 24 +--
.../real_quad/src/hw_impl_zybo.h | 14 +-
.../real_quad/src/hw_impl_zybo_motor.c | 55 +++++++
.../real_quad/src/hw_impl_zybo_pwm_input.c | 32 ----
.../real_quad/src/hw_impl_zybo_pwm_output.c | 54 -------
.../real_quad/src/hw_impl_zybo_rc_receiver.c | 40 +++++
.../real_quad/src/hw_impl_zybo_tests.c | 29 ++--
quad/xsdk_workspace/real_quad/src/main.c | 8 +-
35 files changed, 346 insertions(+), 574 deletions(-)
delete mode 100644 quad/src/quad_app/actuator_command_processing.c
delete mode 100644 quad/src/quad_app/actuator_command_processing.h
rename quad/src/virt_quad/{hw_impl_unix_pwm_output.c => hw_impl_unix_motor.c} (62%)
rename quad/src/virt_quad/{hw_impl_unix_pwm_input.c => hw_impl_unix_rc_receiver.c} (65%)
create mode 100644 quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c
delete mode 100644 quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c
delete mode 100644 quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c
create mode 100644 quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index 922d6439f..9e7d693e6 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -34,34 +34,36 @@ Timeout::timeout(30) {
puts("Beginning tests...")
- # Set gravity
+ # 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
+ check_motors_are_off "Motors weren't off at startup! How dangerous!"
puts("Check that LED is off at startup")
- check_led(0)
+ check_led(0, "LED was on at startup! It should be off so that we can verify when the quad is ready.")
puts("Check that increasing the throttle does nothing to motors")
# (because gear is still off)
- for i in (THROTTLE_MIN..THROTTLE_MAX).step(1000)
+ for i in (THROTTLE_MIN..THROTTLE_MAX).step(0.01)
File.write(THROTTLE, i)
- check_motors_are_off
+ 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
end
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.015
- check_motors_are_off
+ sleep 0.5
+ check_motors_are_off "Motors turned on by gear while rc throttle was high! How dangerous!"
i = THROTTLE_MAX
while i > THROTTLE_MID
- i -= 1000
+ i -= 0.01
File.write(THROTTLE, i)
- check_motors_are_off
- check_led 0
+ 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
@@ -73,8 +75,8 @@ Timeout::timeout(30) {
# (motors should still be off because our throttle is low)
File.write(GEAR, GEAR_ON)
sleep 0.5
- check_motors_are_off
- check_led 1
+ 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)
@@ -87,7 +89,7 @@ Timeout::timeout(30) {
# (and that light goes off)
File.write(GEAR, GEAR_OFF)
sleep 0.5
- check_motors_are_off
+ 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)
diff --git a/quad/scripts/tests/testing_library.rb b/quad/scripts/tests/testing_library.rb
index 4deefed67..5324eacdd 100644
--- a/quad/scripts/tests/testing_library.rb
+++ b/quad/scripts/tests/testing_library.rb
@@ -1,23 +1,23 @@
-THROTTLE_MIN = 110200
-THROTTLE_MAX = 191900
+THROTTLE_MIN = 0.10200
+THROTTLE_MAX = 0.91900
THROTTLE_MID = (THROTTLE_MAX + THROTTLE_MIN)/2
THROTTLE_3_4 = (THROTTLE_MAX + THROTTLE_MID)/2
THROTTLE_QUAR = (THROTTLE_MID + THROTTLE_MIN)/2
THROTTLE_EIGHTH = (THROTTLE_QUAR + THROTTLE_MIN)/2
THROTTLE_16 = (THROTTLE_EIGHTH + THROTTLE_MIN)/2
-MOTOR_MIN = 100000
-MOTOR_MAX = 200000
-GEAR_ON = 170800
-GEAR_OFF = 118300
+MOTOR_MIN = 0.0
+MOTOR_MAX = 1.0
+GEAR_ON = 0.70800
+GEAR_OFF = 0.18300
GRAVITY = 4096
-MOTOR1 = "virt-quad-fifos/pwm-output-motor1"
-MOTOR2 = "virt-quad-fifos/pwm-output-motor2"
-MOTOR3 = "virt-quad-fifos/pwm-output-motor3"
-MOTOR4 = "virt-quad-fifos/pwm-output-motor4"
+MOTOR1 = "virt-quad-fifos/motor1"
+MOTOR2 = "virt-quad-fifos/motor2"
+MOTOR3 = "virt-quad-fifos/motor3"
+MOTOR4 = "virt-quad-fifos/motor4"
-GEAR = "virt-quad-fifos/pwm-input-gear"
-THROTTLE = "virt-quad-fifos/pwm-input-throttle"
+GEAR = "virt-quad-fifos/rc-gear"
+THROTTLE = "virt-quad-fifos/rc-throttle"
LED = "virt-quad-fifos/mio7-led"
@@ -43,15 +43,15 @@ def read_fifo_num(f)
end
# Utility functions
-def check_motors_are_off
+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
- assert_operator motor2, :<=, THROTTLE_MIN
- assert_operator motor3, :<=, THROTTLE_MIN
- assert_operator motor4, :<=, THROTTLE_MIN
+ assert_operator(motor1, :<=, THROTTLE_MIN, msg)
+ assert_operator(motor2, :<=, THROTTLE_MIN, msg)
+ assert_operator(motor3, :<=, THROTTLE_MIN, msg)
+ assert_operator(motor4, :<=, THROTTLE_MIN, msg)
end
def get_motor_averages
@@ -71,9 +71,9 @@ def get_motor_averages
average
end
-def check_led(is_on)
+def check_led(is_on, msg)
led = read_fifo_num(LED)
- assert_equal(is_on, led)
+ assert_equal(is_on, led, msg)
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 40972ad15..046f133ee 100644
--- a/quad/src/graph_blocks/node_mixer.c
+++ b/quad/src/graph_blocks/node_mixer.c
@@ -1,30 +1,30 @@
#include "node_mixer.h"
#include <stdlib.h>
-static int pwm_min = 100000;
-static int pwm_max = 200000;
+static int motor_min = 0.00000;
+static int motor_max = 1.00000;
-static int pwm_clamp(int val) {
- if (val < pwm_min) {val = pwm_min;}
- if (val > pwm_max) {val = pwm_max;}
+static double motor_clamp(double val) {
+ if (val < motor_min) {val = motor_min;}
+ if (val > motor_max) {val = motor_max;}
return val;
}
static void mixer_computation(void *state, const double* params, const double *inputs, double *outputs) {
- int pwm0 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] - inputs[MIXER_ROLL] - inputs[MIXER_YAW];
- int pwm1 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] - inputs[MIXER_ROLL] + inputs[MIXER_YAW];
- int pwm2 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] + inputs[MIXER_ROLL] + inputs[MIXER_YAW];
- int pwm3 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] + inputs[MIXER_ROLL] - inputs[MIXER_YAW];
- outputs[MIXER_PWM0] = pwm_clamp(pwm0);
- outputs[MIXER_PWM1] = pwm_clamp(pwm1);
- outputs[MIXER_PWM2] = pwm_clamp(pwm2);
- outputs[MIXER_PWM3] = pwm_clamp(pwm3);
+ double motor0 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] - inputs[MIXER_ROLL] - inputs[MIXER_YAW];
+ double motor1 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] - inputs[MIXER_ROLL] + inputs[MIXER_YAW];
+ double motor2 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] + inputs[MIXER_ROLL] + inputs[MIXER_YAW];
+ double motor3 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] + inputs[MIXER_ROLL] - inputs[MIXER_YAW];
+ outputs[MIXER_MOTOR0] = motor_clamp(motor0);
+ outputs[MIXER_MOTOR1] = motor_clamp(motor1);
+ outputs[MIXER_MOTOR2] = motor_clamp(motor2);
+ outputs[MIXER_MOTOR3] = motor_clamp(motor3);
}
static void reset_mixer(void *state) {}
static const char* const in_names[4] = {"Throttle", "Pitch", "Roll", "Yaw"};
-static const char* const out_names[4] = {"PWM 0", "PWM 1", "PWM 2", "PWM 3"};
+static const char* const out_names[4] = {"MOTOR 0", "MOTOR 1", "MOTOR 2", "MOTOR 3"};
static const char* const param_names[1] = {"You shouldnt see this"};
const struct graph_node_type node_mixer_type = {
.input_names = in_names,
diff --git a/quad/src/graph_blocks/node_mixer.h b/quad/src/graph_blocks/node_mixer.h
index 374a88be3..c2805fe8d 100644
--- a/quad/src/graph_blocks/node_mixer.h
+++ b/quad/src/graph_blocks/node_mixer.h
@@ -13,10 +13,10 @@ enum graph_node_mixer_inputs {
};
enum graph_node_mixer_outputs {
- MIXER_PWM0,
- MIXER_PWM1,
- MIXER_PWM2,
- MIXER_PWM3,
+ MIXER_MOTOR0,
+ MIXER_MOTOR1,
+ MIXER_MOTOR2,
+ MIXER_MOTOR3,
};
#endif // __NODE_MIXER_H__
diff --git a/quad/src/quad_app/PID.h b/quad/src/quad_app/PID.h
index faf5579f0..9188e0971 100644
--- a/quad/src/quad_app/PID.h
+++ b/quad/src/quad_app/PID.h
@@ -15,7 +15,7 @@
// Yaw constants
// when using units of radians
-#define YAW_ANGULAR_VELOCITY_KP 29700//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KP 0.297
#define YAW_ANGULAR_VELOCITY_KI 0.0f
#define YAW_ANGULAR_VELOCITY_KD 0.0f
#define YAW_ANGLE_KP 2.6f
@@ -25,9 +25,9 @@
// when using units of radians
-#define ROLL_ANGULAR_VELOCITY_KP 3000
+#define ROLL_ANGULAR_VELOCITY_KP 0.03
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
-#define ROLL_ANGULAR_VELOCITY_KD 500
+#define ROLL_ANGULAR_VELOCITY_KD 0.005
#define ROLL_ANGULAR_VELOCITY_ALPHA 0.88
#define ROLL_ANGLE_KP 35
#define ROLL_ANGLE_KI 0.0f
@@ -44,9 +44,9 @@
//Pitch constants
// when using units of radians
-#define PITCH_ANGULAR_VELOCITY_KP 3000
+#define PITCH_ANGULAR_VELOCITY_KP 0.03
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
-#define PITCH_ANGULAR_VELOCITY_KD 500
+#define PITCH_ANGULAR_VELOCITY_KD 0.005
#define PITCH_ANGULAR_VELOCITY_ALPHA 0.88
#define PITCH_ANGLE_KP 35
#define PITCH_ANGLE_KI 0.0f
@@ -62,9 +62,9 @@
//Throttle constants
-#define ALT_ZPOS_KP -9804.0f
-#define ALT_ZPOS_KI -817.0f
-#define ALT_ZPOS_KD -7353.0f
+#define ALT_ZPOS_KP -0.09804f
+#define ALT_ZPOS_KI -0.00817f
+#define ALT_ZPOS_KD -0.07353f
#define ALT_ZPOS_ALPHA 0.88
#endif /* PID_H_ */
diff --git a/quad/src/quad_app/actuator_command_processing.c b/quad/src/quad_app/actuator_command_processing.c
deleted file mode 100644
index e27dbf434..000000000
--- a/quad/src/quad_app/actuator_command_processing.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * actuator_command_processing.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "actuator_command_processing.h"
-#include "sensor_processing.h"
-#include "controllers.h"
-
-int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct)
-{
-
- Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
-// old_Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
-
- return 0;
-}
-
-/**
- * Converts Aero 4 channel signals to PWM signals
- * Aero channels are defined above
- */
-void Aero_to_PWMS(int* PWMs, int* aero)
-{
- int motor0_bias = 0, motor1_bias = 0, motor2_bias = 0, motor3_bias = 0;
- int pwm0 = 0, pwm1 = 0, pwm2 = 0, pwm3 = 0;
-
- pwm0 = aero[THROTTLE] - aero[PITCH] - aero[ROLL] - aero[YAW] + motor0_bias;
- pwm1 = aero[THROTTLE] + aero[PITCH] - aero[ROLL] + aero[YAW] + motor1_bias;
- pwm2 = aero[THROTTLE] - aero[PITCH] + aero[ROLL] + aero[YAW] + motor2_bias;
- pwm3 = aero[THROTTLE] + aero[PITCH] + aero[ROLL] - aero[YAW] + motor3_bias;
-
-// printf("pwm0: %d\tpwm1: %d\tpwm2: %d\tpwm3: %d\n", pwm0, pwm1, pwm2, pwm3);
-
- /**
- * Boundary checks:
- *
- * #define min 100000
- * #define max 200000
- */
-
- if(pwm0 < min)
- pwm0 = min;
- else if(pwm0 > max)
- pwm0 = max;
-
- if(pwm1 < min)
- pwm1 = min;
- else if(pwm1 > max)
- pwm1 = max;
-
- if(pwm2 < min)
- pwm2 = min;
- else if(pwm2 > max)
- pwm2 = max;
-
- if(pwm3 < min)
- pwm3 = min;
- else if(pwm3 > max)
- pwm3 = max;
-
- PWMs[0] = pwm0;
- PWMs[1] = pwm1;
- PWMs[2] = pwm2;
- PWMs[3] = pwm3;
-
- // the array PWMs is then written directly to the PWM hardware registers
- // the PWMs are in units of clock cycles, not percentage duty cycle
- // use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
-}
-
-/**
- * Converts Aero 4 channel signals to PWM signals
- * Aero channels are defined above
- *
- * *deprecated
- */
-void old_Aero_to_PWMS(int* PWMs, int* aero) {
-
- int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
-// int motor0_bias = -5000, motor1_bias = 0, motor2_bias = -5000, motor3_bias = 0;
-
- // Throttle, pitch, roll, yaw as a percentage of their max - Range 0.0 - 100.0
- float throttle_100 = (aero[THROTTLE] - THROTTLE_MIN) / (THROTTLE_RANGE*1.0);
- float pitch_100 = (aero[PITCH] - PITCH_MIN) / (PITCH_RANGE*1.0);
- float roll_100 = (aero[ROLL] - ROLL_MIN) / (ROLL_RANGE*1.0);
- float yaw_100 = (aero[YAW] - YAW_MIN) / (YAW_RANGE*1.0);
-
- // This adds a +/- 300 ms range bias for the throttle
- int throttle_base = BASE + (int) 60000 * (throttle_100 - .5);
- // This adds a +/- 200 ms range bias for the pitch
- int pitch_base = (int) 60000 * (pitch_100 - .5);
- // This adds a +/- 200 ms range bias for the roll
- int roll_base = (int) 60000 * (roll_100 - .5);
- // This adds a +/- 75 ms range bias for the yaw
- int yaw_base = (int) 15000 * (yaw_100 - .5);
-
- int pwm0, pwm1, pwm2, pwm3;
-
- pwm1 = throttle_base + pitch_base/2 - roll_base/2 + yaw_base + motor1_bias;
- pwm3 = throttle_base + pitch_base/2 + roll_base/2 - yaw_base + motor3_bias;
- pwm0 = throttle_base - pitch_base/2 - roll_base/2 - yaw_base + motor0_bias;
- pwm2 = throttle_base - pitch_base/2 + roll_base/2 + yaw_base + motor2_bias;
-
- /**
- * Boundary checks:
- *
- * #define min 100000
- * #define max 200000
- */
-
- if(pwm0 < min)
- pwm0 = min;
- else if(pwm0 > max)
- pwm0 = max;
-
- if(pwm1 < min)
- pwm1 = min;
- else if(pwm1 > max)
- pwm1 = max;
-
- if(pwm2 < min)
- pwm2 = min;
- else if(pwm2 > max)
- pwm2 = max;
-
- if(pwm3 < min)
- pwm3 = min;
- else if(pwm3 > max)
- pwm3 = max;
-
- PWMs[0] = pwm0;
- PWMs[1] = pwm1;
- PWMs[2] = pwm2;
- PWMs[3] = pwm3;
-
- // the array PWMs is then written directly to the PWM hardware registers
- // the PWMs are in units of clock cycles, not percentage duty cycle
- // use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
-
-}
diff --git a/quad/src/quad_app/actuator_command_processing.h b/quad/src/quad_app/actuator_command_processing.h
deleted file mode 100644
index 12110d551..000000000
--- a/quad/src/quad_app/actuator_command_processing.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * actuator_command_processing.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef ACTUATOR_COMMAND_PROCESSING_H_
-#define ACTUATOR_COMMAND_PROCESSING_H_
-
-#include <stdio.h>
-
-#include "log_data.h"
-#include "control_algorithm.h"
-
-/**
- * @brief
- * Processes the commands to the actuators.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @param raw_actuator_struct
- * structure of the commmands outputted to go to the actuators
- *
- * @param actuator_command_struct
- * structure of the commmands to go to the actuators
- *
- * @return
- * error message
- *
- */
-int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct);
-
-void old_Aero_to_PWMS(int* PWMs, int* aero);
-void Aero_to_PWMS(int* PWMs, int* aero);
-
-#endif /* ACTUATOR_COMMAND_PROCESSING_H_ */
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 7d867cbfc..845c58433 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -376,22 +376,22 @@ int control_algorithm_init(parameter_t * ps)
// don't use the PID corrections if the throttle is less than about 10% of its range
// unless we are in autonomous
- if((user_input_struct->rc_commands[THROTTLE] > 118000) ||
+ if((user_input_struct->rc_commands[THROTTLE] > 0.18000) ||
user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
{
//THROTTLE
- actuator_struct->pwms[0] = graph_get_output(graph, ps->mixer, MIXER_PWM0);
- actuator_struct->pwms[1] = graph_get_output(graph, ps->mixer, MIXER_PWM1);
- actuator_struct->pwms[2] = graph_get_output(graph, ps->mixer, MIXER_PWM2);
- actuator_struct->pwms[3] = graph_get_output(graph, ps->mixer, MIXER_PWM3);
+ actuator_struct->motor_magnitudes[0] = graph_get_output(graph, ps->mixer, MIXER_MOTOR0);
+ actuator_struct->motor_magnitudes[1] = graph_get_output(graph, ps->mixer, MIXER_MOTOR1);
+ actuator_struct->motor_magnitudes[2] = graph_get_output(graph, ps->mixer, MIXER_MOTOR2);
+ actuator_struct->motor_magnitudes[3] = graph_get_output(graph, ps->mixer, MIXER_MOTOR3);
}
else
{
- actuator_struct->pwms[0] = user_input_struct->rc_commands[THROTTLE];
- actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE];
- actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE];
- actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[0] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[1] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[2] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[3] = user_input_struct->rc_commands[THROTTLE];
}
last_fm_switch = cur_fm_switch;
diff --git a/quad/src/quad_app/controllers.h b/quad/src/quad_app/controllers.h
index 1e12c7590..40c783260 100644
--- a/quad/src/quad_app/controllers.h
+++ b/quad/src/quad_app/controllers.h
@@ -71,39 +71,41 @@
/**
* Signals from the Rx mins, maxes and ranges
*/
-#define THROTTLE_MAX 191900
-#define THROTTLE_MIN 110200
+#define THROTTLE_MAX 0.91900
+#define THROTTLE_MIN 0.10200
#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-#define ROLL_MAX 170200
-#define ROLL_MIN 129400
-#define ROLL_CENTER 149800
+#define ROLL_MAX 0.70200
+#define ROLL_MIN 0.29400
+#define ROLL_CENTER 0.49800
#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-#define PITCH_MAX 169900
-#define PITCH_MIN 129500
-#define PITCH_CENTER 149700
+#define PITCH_MAX 0.69900
+#define PITCH_MIN 0.29500
+#define PITCH_CENTER 0.49700
#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-#define YAW_MAX 169400
-#define YAW_MIN 129300
-#define YAW_CENTER 149800
+#define YAW_MAX 0.69400
+#define YAW_MIN 0.29300
+#define YAW_CENTER 0.49800
#define YAW_RANGE YAW_MAX - YAW_MIN
-#define GEAR_1 170800
-#define GEAR_0 118300
+#define GEAR_1 0.70800
+#define GEAR_0 0.18300
+#define GEAR_MID (GEAR_0 + GEAR_1)/2.0
-#define FLAP_1 192000
-#define FLAP_0 107600
+#define FLAP_1 0.92000
+#define FLAP_0 0.07600
+#define FLAP_MID (FLAP_0 + FLAP_1)/2.0
#define GEAR_KILL GEAR_0 // The kill point for the program
-#define BASE 150000
+#define BASE 0.50000
-#define min 100000
-#define max 200000
+#define min 0.00000
+#define max 1.00000
-#define MOTOR_MIN 100000
-#define MOTOR_MAX 200000
+#define MOTOR_MIN 0.00000
+#define MOTOR_MAX 1.00000
void filter_PWMs(int* mixer);
void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index 48b3d0cf7..ca787b9fc 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/quad/src/quad_app/hw_iface.h
@@ -28,16 +28,16 @@ struct I2CDriver {
unsigned int length);
};
-struct PWMOutputDriver {
+struct RCReceiverDriver {
void *state;
- int (*reset)(struct PWMOutputDriver *self);
- int (*write)(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+ int (*reset)(struct RCReceiverDriver *self);
+ int (*read)(struct RCReceiverDriver *self, unsigned int channel, float *magnitude);
};
-struct PWMInputDriver {
+struct MotorDriver {
void *state;
- int (*reset)(struct PWMInputDriver *self);
- int (*read)(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+ int (*reset)(struct MotorDriver *self);
+ int (*write)(struct MotorDriver *self, unsigned int channel, float magnitude);
};
struct UARTDriver {
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index e476fd633..5cd49342d 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -17,18 +17,18 @@ extern int Xil_AssertWait;
int protection_loops(modular_structs_t *structs)
{
- u32 rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
+ float rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
- struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
- read_rec_all(pwm_inputs, rc_commands);
+ struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
+ read_rec_all(rc_receiver, rc_commands);
- while(rc_commands[THROTTLE] < 75000 || // wait for RC receiver to connect to transmitter
- rc_commands[THROTTLE] > 125000 || // Wait until throttle is low
+ 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(pwm_inputs, rc_commands);
+ read_rec_all(rc_receiver, rc_commands);
}
// let the pilot/observers know that the quad is now active
@@ -77,15 +77,15 @@ int init_structs(modular_structs_t *structs) {
iic_set_globals(i2c, sys);
// Initialize PWM Recorders and Motor outputs
- struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
- if (pwm_inputs->reset(pwm_inputs)) return -1;
- struct PWMOutputDriver *pwm_outputs = &structs->hardware_struct.pwm_outputs;
- if (pwm_outputs->reset(pwm_outputs)) return -1;
+ struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
+ if (rc_receiver->reset(rc_receiver)) return -1;
+ struct MotorDriver *motors = &structs->hardware_struct.motors;
+ if (motors->reset(motors)) return -1;
// Set motor outputs to off
int i;
for (i = 0; i < 4; i += 1) {
- pwm_outputs->write(pwm_outputs, i, MOTOR_MIN);
+ motors->write(motors, i, MOTOR_MIN);
}
// Initialize sensors
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 5714d32a2..bdc38f0ee 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -117,10 +117,10 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM0, pwm_val);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM1, pwm_val);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM2, pwm_val);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM3, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR0, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR1, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR2, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR3, pwm_val);
addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 01eabe901..f83cc8698 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -12,7 +12,6 @@
#include "sensor.h"
#include "sensor_processing.h"
#include "control_algorithm.h"
-#include "actuator_command_processing.h"
#include "send_actuator_commands.h"
#include "update_gui.h"
#include "communication.h"
@@ -67,10 +66,10 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
// send the actuator commands
- send_actuator_commands(&(structs.hardware_struct.pwm_outputs), &(structs.log_struct), &(structs.actuator_command_struct));
+ send_actuator_commands(&(structs.hardware_struct.motors), &(structs.log_struct), &(structs.actuator_command_struct));
}
else {
- kill_motors(&(structs.hardware_struct.pwm_outputs));
+ kill_motors(&(structs.hardware_struct.motors));
}
// update the GUI
update_GUI(&(structs.log_struct));
@@ -115,7 +114,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
timer_end_loop(&(structs.log_struct));
}
- kill_motors(&(structs.hardware_struct.pwm_outputs));
+ kill_motors(&(structs.hardware_struct.motors));
flash_MIO_7_led(10, 100);
diff --git a/quad/src/quad_app/send_actuator_commands.c b/quad/src/quad_app/send_actuator_commands.c
index c361e74db..8120224d0 100644
--- a/quad/src/quad_app/send_actuator_commands.c
+++ b/quad/src/quad_app/send_actuator_commands.c
@@ -8,12 +8,12 @@
#include "send_actuator_commands.h"
#include "util.h"
-int send_actuator_commands(struct PWMOutputDriver *pwm_outputs, log_t* log_struct, actuator_command_t* actuator_command_struct) {
+int send_actuator_commands(struct MotorDriver *motors, log_t* log_struct, actuator_command_t* actuator_command_struct) {
int i;
// write the PWMs to the motors
for (i = 0; i < 4; i++) {
- pwm_outputs->write(pwm_outputs, i, actuator_command_struct->pwms[i]);
+ motors->write(motors, i, actuator_command_struct->motor_magnitudes[i]);
}
return 0;
diff --git a/quad/src/quad_app/send_actuator_commands.h b/quad/src/quad_app/send_actuator_commands.h
index 3f6fb0fc8..747ba0479 100644
--- a/quad/src/quad_app/send_actuator_commands.h
+++ b/quad/src/quad_app/send_actuator_commands.h
@@ -11,7 +11,6 @@
#include <stdio.h>
#include "log_data.h"
-#include "actuator_command_processing.h"
/**
* @brief
@@ -27,6 +26,6 @@
* error message
*
*/
-int send_actuator_commands(struct PWMOutputDriver *pwm_outputs, log_t* log_struct, actuator_command_t* actuator_command_struct);
+int send_actuator_commands(struct MotorDriver *motors, log_t* log_struct, actuator_command_t* actuator_command_struct);
#endif /* SEND_ACTUATOR_COMMANDS_H_ */
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index 2d9396de9..b2f7592c8 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -12,9 +12,9 @@
int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
{
- if (iic0_lidarlite_init()) { // init LiDAR
- return -1;
- }
+// if (iic0_lidarlite_init()) { // init LiDAR
+// return -1;
+// }
if(iic0_mpu9150_start() == -1) {
return -1;
@@ -74,9 +74,9 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
log_struct->gam = raw_sensor_struct->gam;
- static lidar_t lidar_val;
- iic0_lidarlite_read_distance(&lidar_val);
- raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+// static lidar_t lidar_val;
+// iic0_lidarlite_read_distance(&lidar_val);
+// raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
return 0;
}
diff --git a/quad/src/quad_app/sensor_processing.h b/quad/src/quad_app/sensor_processing.h
index 52b626eaf..d340e3458 100644
--- a/quad/src/quad_app/sensor_processing.h
+++ b/quad/src/quad_app/sensor_processing.h
@@ -19,45 +19,6 @@
#define GEAR 4
#define FLAP 5
-/**
- * Signals from the Rx mins, maxes and ranges
- */
-//#define THROTTLE_MAX 191900
-//#define THROTTLE_MIN 110200
-//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-//
-//#define ROLL_MAX 170200
-////#define ROLL_MAX 167664
-////#define ROLL_CENTER 148532
-//#define ROLL_MIN 129400
-//#define ROLL_CENTER 149800
-//#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-//
-////#define PITCH_MAX 190800
-//#define PITCH_MAX 169900
-////#define PITCH_MIN 135760
-//#define PITCH_MIN 129500
-////#define PITCH_CENTER 152830
-//#define PITCH_CENTER 149700
-//#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-//
-//#define YAW_MAX 169400
-//#define YAW_MIN 129300
-//#define YAW_CENTER 149800
-//#define YAW_RANGE YAW_MAX - YAW_MIN
-//
-//#define GEAR_1 170800
-//#define GEAR_0 118300
-//
-//#define FLAP_1 192000
-//#define FLAP_0 107600
-//
-//#define GEAR_KILL GEAR_0 // The kill point for the program
-//#define BASE 150000
-//
-//#define min 100000
-//#define max 200000
-
/**
* @brief
* Processes the data from the sensors.
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index b7e398b6b..e2e40c1aa 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -148,7 +148,7 @@ typedef struct PID_values{
*
*/
typedef struct user_input_t {
- u32 rc_commands[6]; // Commands from the RC transmitter
+ float rc_commands[6]; // Commands from the RC transmitter
// float cam_x_pos; // Current x position from the camera system
@@ -390,7 +390,7 @@ typedef struct user_defined_t {
*/
typedef struct raw_actuator_t {
- int controller_corrected_motor_commands[6];
+ float controller_corrected_motor_commands[6];
} raw_actuator_t;
@@ -400,7 +400,7 @@ typedef struct raw_actuator_t {
*
*/
typedef struct actuator_command_t {
- int pwms[4];
+ float motor_magnitudes[4];
} actuator_command_t;
enum PWMChannels {
@@ -418,8 +418,8 @@ enum PWMChannels {
typedef struct hardware_t {
struct I2CDriver i2c;
- struct PWMInputDriver pwm_inputs;
- struct PWMOutputDriver pwm_outputs;
+ struct RCReceiverDriver rc_receiver;
+ struct MotorDriver motors;
struct UARTDriver uart;
struct TimerDriver global_timer;
struct TimerDriver axi_timer;
diff --git a/quad/src/quad_app/user_input.c b/quad/src/quad_app/user_input.c
index bcb7478bf..824910a1b 100644
--- a/quad/src/quad_app/user_input.c
+++ b/quad/src/quad_app/user_input.c
@@ -10,8 +10,8 @@
int get_user_input(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct)
{
// Read in values from RC Receiver
- struct PWMInputDriver *pwm_inputs = &hardware_struct->pwm_inputs;
- read_rec_all(pwm_inputs, user_input_struct->rc_commands);
+ struct RCReceiverDriver *rc_receiver = &hardware_struct->rc_receiver;
+ read_rec_all(rc_receiver, user_input_struct->rc_commands);
//create setpoints for manual flight
// currently in units of radians
@@ -44,10 +44,10 @@ int kill_condition(user_input_t* user_input_struct)
* -
*
*/
-float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target)
+float convert_from_receiver_cmd(float receiver_cmd, float max_receiver_cmd, float center_receiver_cmd, float min_receiver_cmd, float max_target, float min_target)
{
// centers the receiver command by subtracting the given center value. This means that if receiver_cmd == center then receiver_cmd_centered should be 0.
- int receiver_cmd_centered = receiver_cmd - center_receiver_cmd;
+ float receiver_cmd_centered = receiver_cmd - center_receiver_cmd;
if(receiver_cmd_centered <= 0) {
float ret = ((float)(receiver_cmd_centered * min_target)) / ((float) (min_receiver_cmd - center_receiver_cmd));
diff --git a/quad/src/quad_app/user_input.h b/quad/src/quad_app/user_input.h
index c289e99c9..674f66e6e 100644
--- a/quad/src/quad_app/user_input.h
+++ b/quad/src/quad_app/user_input.h
@@ -35,36 +35,6 @@
#define PITCH_DEG_TARGET 12.0f
#define PITCH_RAD_TARGET ((float) ((PITCH_DEG_TARGET * 3.141592) / ((float) 180)))
-/////// Signals from the Rx mins, maxes and ranges
-
-//#define THROTTLE_MAX 191900
-//#define THROTTLE_MIN 110200
-//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-//
-//#define ROLL_MAX 170200
-//#define ROLL_MIN 129400
-//#define ROLL_CENTER 149800
-//#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-//
-//#define PITCH_MAX 169900
-//#define PITCH_MIN 129500
-//#define PITCH_CENTER 149700
-//#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-//
-//#define YAW_MAX 169400
-//#define YAW_MIN 129300
-//#define YAW_CENTER (YAW_MIN + YAW_MAX)/2 //149800
-//#define YAW_RANGE YAW_MAX - YAW_MIN
-//
-//#define GEAR_1 170800
-//#define GEAR_0 118300
-//
-//#define FLAP_1 192000
-//#define FLAP_0 107600
-//
-//#define GEAR_KILL GEAR_0 // The kill point for the program
-//#define BASE 150000
-
/**
* @brief
* Receives user input to the system.
@@ -81,7 +51,7 @@
*/
int get_user_input(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct);
int kill_condition(user_input_t* user_input_struct);
-float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target);
+float convert_from_receiver_cmd(float receiver_cmd, float max_receiver_cmd, float center_receiver_cmd, float min_receiver_cmd, float max_target, float min_target);
#endif /* USER_INPUT_H_ */
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index 2f1512a0e..e982432ea 100644
--- a/quad/src/quad_app/util.c
+++ b/quad/src/quad_app/util.c
@@ -13,10 +13,10 @@ extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;
/**
* Reads all 6 receiver channels at once
*/
-void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer){
+void read_rec_all(struct RCReceiverDriver *rc_receiver, float *mixer){
int i;
for(i = 0; i < 6; i++){
- pwm_input->read(pwm_input, i, &mixer[i]);
+ rc_receiver->read(rc_receiver, i, &mixer[i]);
}
}
@@ -48,30 +48,37 @@ int hexStrToInt(char *buf, int startIdx, int endIdx) {
* If the message from the receiver is 0 - gear, kill the system by sending a 1
* Otherwise, do nothing
*/
-int read_kill(int gear){
- if(gear > 115000 && gear < 125000)
+int read_kill(float gear) {
+ if(gear < GEAR_MID)
return 1;
return 0;
}
-int read_flap(int flap)
-{
- // flap '0' is 108,000 CC (Up)
- // flap '1' is 192,000 CC (Down)
- // here we say if the reading is greater than 150,000 than its '1'; '0' otherwise
- if(flap > 150000)
+int read_flap(float flap) {
+ if(flap > FLAP_MID)
return 1;
return 0;
}
+/**
+ * Return true if the transmitter is on. This is mostly a failsafe to be sure we don't
+ * drop out of the air if the receiver cuts out momentarily. If the receiver
+ * cuts out, we expect the rc values will flatline at 0. So if we get a float zero,
+ * then assume the transmitter is disconnected.
+ */
+int is_transmitter_on(float gear) {
+ if (gear < 0.00001) return 0;
+ else return 1;
+}
+
/**
* Turns off the motors
*/
-void kill_motors(struct PWMOutputDriver *pwm_outputs) {
- pwm_outputs->write(pwm_outputs, 0, MOTOR_MIN);
- pwm_outputs->write(pwm_outputs, 1, MOTOR_MIN);
- pwm_outputs->write(pwm_outputs, 2, MOTOR_MIN);
- pwm_outputs->write(pwm_outputs, 3, MOTOR_MIN);
+void kill_motors(struct MotorDriver *motors) {
+ motors->write(motors, 0, MOTOR_MIN);
+ motors->write(motors, 1, MOTOR_MIN);
+ motors->write(motors, 2, MOTOR_MIN);
+ motors->write(motors, 3, MOTOR_MIN);
}
int build_int(u8 *buff) {
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index 643498afc..390bafd9d 100644
--- a/quad/src/quad_app/util.h
+++ b/quad/src/quad_app/util.h
@@ -11,10 +11,10 @@
#include "controllers.h"
#include "hw_iface.h"
-void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer);
-int read_kill(int gear);
-int read_flap(int flap);
-void kill_motors(struct PWMOutputDriver *pwm_outputs);
+void read_rec_all(struct RCReceiverDriver *rc_receiver, float *mixer);
+int read_kill(float gear);
+int read_flap(float flap);
+void kill_motors(struct MotorDriver *motors);
int build_int(u8 *buff);
float build_float(u8 *buff);
diff --git a/quad/src/virt_quad/hw_impl_unix.c b/quad/src/virt_quad/hw_impl_unix.c
index fea231c35..bbd8e8775 100644
--- a/quad/src/virt_quad/hw_impl_unix.c
+++ b/quad/src/virt_quad/hw_impl_unix.c
@@ -9,20 +9,20 @@ struct UARTDriver create_unix_uart() {
return uart;
}
-struct PWMOutputDriver create_unix_pwm_outputs() {
- struct PWMOutputDriver pwm_outputs;
- pwm_outputs.state = NULL;
- pwm_outputs.reset = unix_pwm_output_reset;
- pwm_outputs.write = unix_pwm_output_write;
- return pwm_outputs;
+struct MotorDriver create_unix_motors() {
+ struct MotorDriver motors;
+ motors.state = NULL;
+ motors.reset = unix_motor_reset;
+ motors.write = unix_motor_write;
+ return motors;
}
-struct PWMInputDriver create_unix_pwm_inputs() {
- struct PWMInputDriver pwm_inputs;
- pwm_inputs.state = NULL;
- pwm_inputs.reset = unix_pwm_input_reset;
- pwm_inputs.read = unix_pwm_input_read;
- return pwm_inputs;
+struct RCReceiverDriver create_unix_rc_receiver() {
+ struct RCReceiverDriver rc_receivers;
+ rc_receivers.state = NULL;
+ rc_receivers.reset = unix_rc_receiver_reset;
+ rc_receivers.read = unix_rc_receiver_read;
+ return rc_receivers;
}
struct I2CDriver create_unix_i2c() {
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index 387458993..85e223f28 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -16,11 +16,11 @@ 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);
-int unix_pwm_output_reset(struct PWMOutputDriver *self);
-int unix_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+int unix_motor_reset(struct MotorDriver *self);
+int unix_motor_write(struct MotorDriver *self, unsigned int channel, float magnitude);
-int unix_pwm_input_reset(struct PWMInputDriver *self);
-int unix_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+int unix_rc_receiver_reset(struct RCReceiverDriver *self);
+int unix_rc_receiver_read(struct RCReceiverDriver *self, unsigned int channel, float *magnitude);
int unix_i2c_reset(struct I2CDriver *self);
int unix_i2c_write(struct I2CDriver *self,
@@ -48,8 +48,8 @@ int unix_system_reset(struct SystemDriver *self);
int unix_system_sleep(struct SystemDriver *self, unsigned long us);
struct UARTDriver create_unix_uart();
-struct PWMOutputDriver create_unix_pwm_outputs();
-struct PWMInputDriver create_unix_pwm_inputs();
+struct MotorDriver create_unix_motors();
+struct RCReceiverDriver create_unix_rc_receiver();
struct I2CDriver create_unix_i2c();
struct TimerDriver create_unix_global_timer();
struct TimerDriver create_unix_axi_timer();
@@ -58,6 +58,6 @@ struct SystemDriver create_unix_system();
int test_unix_i2c();
int test_unix_mio7_led_and_system();
-int test_unix_pwm_inputs();
+int test_unix_rc_receivers();
#endif
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_output.c b/quad/src/virt_quad/hw_impl_unix_motor.c
similarity index 62%
rename from quad/src/virt_quad/hw_impl_unix_pwm_output.c
rename to quad/src/virt_quad/hw_impl_unix_motor.c
index e58d5cf5a..3ad271b52 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_output.c
+++ b/quad/src/virt_quad/hw_impl_unix_motor.c
@@ -9,19 +9,20 @@
void * output_cache();
static char *output_pwms[4];
-static unsigned long cache[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;
-int unix_pwm_output_reset(struct PWMOutputDriver *self) {
- output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor1";
- output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor2";
- output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor3";
- output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor4";
+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);
@@ -34,13 +35,13 @@ int unix_pwm_output_reset(struct PWMOutputDriver *self) {
return 0;
}
-int unix_pwm_output_write(struct PWMOutputDriver *self,
+int unix_motor_write(struct MotorDriver *self,
unsigned int channel,
- unsigned long pulse_width_us) {
- if (cache[channel] != pulse_width_us) {
- printf("%s: %ld\n", output_pwms[channel], pulse_width_us);
+ float magnitude) {
+ if (cache[channel] != magnitude) {
+ printf("%s: %.2f\n", output_pwms[channel], magnitude);
}
- cache[channel] = pulse_width_us;
+ cache[channel] = magnitude;
return 0;
}
@@ -56,10 +57,14 @@ void * output_cache(void *arg) {
// Block while waiting for someone to listen
while (1) {
int fifo = open(output_pwms[i], O_WRONLY);
- sprintf(buff, "%ld\n", cache[i]);
+ 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_pwm_input.c b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
similarity index 65%
rename from quad/src/virt_quad/hw_impl_unix_pwm_input.c
rename to quad/src/virt_quad/hw_impl_unix_rc_receiver.c
index df1aaf5f9..229a019cd 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_input.c
+++ b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
@@ -9,17 +9,17 @@ void * update_input_cache();
static char *input_names[6];
static int fifos[6];
-static unsigned long cache[6];
+static float cache[6];
static pthread_t workers[6];
static int nums[] = {0, 1, 2, 3, 4, 5};
-int unix_pwm_input_reset(struct PWMInputDriver *self) {
- input_names[0] = "pwm-input-throttle";
- input_names[1] = "pwm-input-roll";
- input_names[2] = "pwm-input-pitch";
- input_names[3] = "pwm-input-yaw";
- input_names[4] = "pwm-input-gear";
- input_names[5] = "pwm-input-flap";
+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);
@@ -38,17 +38,17 @@ int unix_pwm_input_reset(struct PWMInputDriver *self) {
cache[5] = FLAP_1;
for (i = 0; i < 6; i += 1) {
- printf("%s: %lu\n", input_names[i], cache[i]);
+ printf("%s: %.2f\n", input_names[i], cache[i]);
}
return 0;
}
-int unix_pwm_input_read(struct PWMInputDriver *self,
+int unix_rc_receiver_read(struct RCReceiverDriver *self,
unsigned int channel,
- unsigned long *pulse_width_us) {
+ float *magnitude) {
- *pulse_width_us = cache[channel];
+ *magnitude = cache[channel];
return 0;
}
@@ -69,13 +69,13 @@ void * update_input_cache(void *arg) {
int bytes_read = read(fifos[i], buff, 15);
if (bytes_read > 0) {
buff[bytes_read] = '\0';
- unsigned long val = strtoll(buff, NULL, 10);
- if (val < max && val > min) {
+ float val = strtof(buff, NULL);
+ if (val <= 1 && val >= 0) {
cache[i] = val;
- printf("%s: %lu\n", input_names[i], val);
+ printf("%s: %.2f\n", input_names[i], val);
}
else {
- printf("%s: Bad value - input not received\n", input_names[i]);
+ printf("%s: Bad value (%f) - input not received\n", input_names[i], val);
}
}
pthread_yield();
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
index f703f6ab7..5f6aaf8db 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -6,8 +6,8 @@
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_unix_i2c();
- hardware->pwm_inputs = create_unix_pwm_inputs();
- hardware->pwm_outputs = create_unix_pwm_outputs();
+ hardware->rc_receiver = create_unix_rc_receiver();
+ hardware->motors = create_unix_motors();
hardware->uart = create_unix_uart();
hardware->global_timer = create_unix_global_timer();
hardware->axi_timer = create_unix_axi_timer();
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 e8fceffa6..3a6348f9b 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
@@ -9,20 +9,20 @@ struct UARTDriver create_zybo_uart() {
return uart;
}
-struct PWMOutputDriver create_zybo_pwm_outputs() {
- struct PWMOutputDriver pwm_outputs;
- pwm_outputs.state = NULL;
- pwm_outputs.reset = zybo_pwm_output_reset;
- pwm_outputs.write = zybo_pwm_output_write;
- return pwm_outputs;
+struct MotorDriver create_zybo_motors() {
+ struct MotorDriver motors;
+ motors.state = NULL;
+ motors.reset = zybo_motor_reset;
+ motors.write = zybo_motor_write;
+ return motors;
}
-struct PWMInputDriver create_zybo_pwm_inputs() {
- struct PWMInputDriver pwm_inputs;
- pwm_inputs.state = NULL;
- pwm_inputs.reset = zybo_pwm_input_reset;
- pwm_inputs.read = zybo_pwm_input_read;
- return pwm_inputs;
+struct RCReceiverDriver create_zybo_rc_receiver() {
+ struct RCReceiverDriver rc_receiver;
+ rc_receiver.state = NULL;
+ rc_receiver.reset = zybo_rc_receiver_reset;
+ rc_receiver.read = zybo_rc_receiver_read;
+ return rc_receiver;
}
struct I2CDriver create_zybo_i2c() {
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 c721307bc..29d0ff8f3 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
@@ -22,11 +22,11 @@ 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);
-int zybo_pwm_output_reset(struct PWMOutputDriver *self);
-int zybo_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+int zybo_motor_reset(struct MotorDriver *self);
+int zybo_motor_write(struct MotorDriver *self, unsigned int channel, float magnitude);
-int zybo_pwm_input_reset(struct PWMInputDriver *self);
-int zybo_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+int zybo_rc_receiver_reset(struct RCReceiverDriver *self);
+int zybo_rc_receiver_read(struct RCReceiverDriver *self, unsigned int channel, float *magnitude);
int zybo_i2c_reset(struct I2CDriver *self);
int zybo_i2c_write(struct I2CDriver *self,
@@ -54,8 +54,8 @@ int zybo_system_reset(struct SystemDriver *self);
int zybo_system_sleep(struct SystemDriver *self, unsigned long us);
struct UARTDriver create_zybo_uart();
-struct PWMOutputDriver create_zybo_pwm_outputs();
-struct PWMInputDriver create_zybo_pwm_inputs();
+struct MotorDriver create_zybo_motors();
+struct RCReceiverDriver create_zybo_rc_receiver();
struct I2CDriver create_zybo_i2c();
struct TimerDriver create_zybo_global_timer();
struct TimerDriver create_zybo_axi_timer();
@@ -64,6 +64,6 @@ struct SystemDriver create_zybo_system();
int test_zybo_i2c();
int test_zybo_mio7_led_and_system();
-int test_zybo_pwm_inputs();
+int test_zybo_rc_receivers();
#endif
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c
new file mode 100644
index 000000000..576c1ecaf
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c
@@ -0,0 +1,55 @@
+#include "hw_impl_zybo.h"
+
+#define SYS_CLOCK_RATE 100000000 // ticks per second
+#define FREQUENCY 450
+#define PERIOD_WIDTH SYS_CLOCK_RATE/FREQUENCY
+#define PULSE_WIDTH_LOW SYS_CLOCK_RATE/1000 // 1 ms
+#define PULSE_WIDTH_HIGH SYS_CLOCK_RATE/500 // 2 ms
+#define PULSE_WIDTH_ADDR_OFFSET 1
+
+struct MotorDriverState {
+ int *outputs[4];
+};
+
+int zybo_motor_reset(struct MotorDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(struct MotorDriverState));
+ if (self->state == NULL) {
+ return -1;
+ }
+ }
+ struct MotorDriverState *state = self->state;
+
+ state->outputs[0] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR;
+ state->outputs[1] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR;
+ state->outputs[2] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR;
+ state->outputs[3] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR;
+
+ // Set period width of PWM pulse
+ *(state->outputs[0]) = PERIOD_WIDTH;
+ *(state->outputs[1]) = PERIOD_WIDTH;
+ *(state->outputs[2]) = PERIOD_WIDTH;
+ *(state->outputs[3]) = PERIOD_WIDTH;
+
+ // Set a low pulse (1 ms) so that outputs are off
+ *(state->outputs[0] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+ *(state->outputs[1] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+ *(state->outputs[2] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+ *(state->outputs[3] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+
+ usleep(1000000);
+ return 0;
+}
+
+int zybo_motor_write(struct MotorDriver *self,
+ unsigned int channel,
+ float magnitude) {
+ if (magnitude > 1)
+ magnitude = 1;
+ if (magnitude < 0)
+ magnitude = 0;
+ struct MotorDriverState *state = self->state;
+ unsigned long pulse_width_ticks = (unsigned long) (magnitude * (float) (PULSE_WIDTH_HIGH - PULSE_WIDTH_LOW)) + PULSE_WIDTH_LOW;
+ *(state->outputs[channel] + PULSE_WIDTH_ADDR_OFFSET) = pulse_width_ticks;
+ return 0;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c
deleted file mode 100644
index b0c3cd98e..000000000
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c
+++ /dev/null
@@ -1,32 +0,0 @@
-#include "hw_impl_zybo.h"
-
-struct PWMInputDriverState {
- int *channels[6];
-};
-
-int zybo_pwm_input_reset(struct PWMInputDriver *self) {
- if (self->state == NULL) {
- self->state = malloc(sizeof(struct PWMInputDriverState));
- if (self->state == NULL) {
- return -1;
- }
- }
-
- // Save the addresses of the input PWM recorders
- struct PWMInputDriverState *state = self->state;
- state->channels[0] = (int *) XPAR_PWM_RECORDER_0_BASEADDR;
- state->channels[1] = (int *) XPAR_PWM_RECORDER_1_BASEADDR;
- state->channels[2] = (int *) XPAR_PWM_RECORDER_2_BASEADDR;
- state->channels[3] = (int *) XPAR_PWM_RECORDER_3_BASEADDR;
- state->channels[4] = (int *) XPAR_PWM_RECORDER_4_BASEADDR;
- state->channels[5] = (int *) XPAR_PWM_RECORDER_5_BASEADDR;
- return 0;
-}
-
-int zybo_pwm_input_read(struct PWMInputDriver *self,
- unsigned int channel,
- unsigned long *pulse_width_us) {
- struct PWMInputDriverState *state = self->state;
- *pulse_width_us = (long) *((int *) state->channels[channel]);
- return 0;
-}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c
deleted file mode 100644
index 3dd09e543..000000000
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c
+++ /dev/null
@@ -1,54 +0,0 @@
-#include "hw_impl_zybo.h"
-
-#define SYS_CLOCK_RATE 100000000 // ticks per second
-#define FREQUENCY 450
-#define PERIOD_WIDTH SYS_CLOCK_RATE/FREQUENCY
-#define THROTTLE_PULSE_WIDTH_LOW SYS_CLOCK_RATE/1000 // 1 ms
-#define THROTTLE_PULSE_WIDTH_HIGH SYS_CLOCK_RATE/500 // 2 ms
-#define PULSE_WIDTH_ADDR_OFFSET 1
-
-struct PWMOutputDriverState {
- int *outputs[4];
-};
-
-int zybo_pwm_output_reset(struct PWMOutputDriver *self) {
- if (self->state == NULL) {
- self->state = malloc(sizeof(struct PWMOutputDriverState));
- if (self->state == NULL) {
- return -1;
- }
- }
- struct PWMOutputDriverState *state = self->state;
-
- state->outputs[0] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR;
- state->outputs[1] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR;
- state->outputs[2] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR;
- state->outputs[3] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR;
-
- // Set period width of PWM pulse
- *(state->outputs[0]) = PERIOD_WIDTH;
- *(state->outputs[1]) = PERIOD_WIDTH;
- *(state->outputs[2]) = PERIOD_WIDTH;
- *(state->outputs[3]) = PERIOD_WIDTH;
-
- // Set a low pulse (1 ms) so that outputs are off
- *(state->outputs[0] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
- *(state->outputs[1] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
- *(state->outputs[2] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
- *(state->outputs[3] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
-
- usleep(1000000);
- return 0;
-}
-
-int zybo_pwm_output_write(struct PWMOutputDriver *self,
- unsigned int channel,
- unsigned long pulse_width_us) {
- if (pulse_width_us > THROTTLE_PULSE_WIDTH_HIGH)
- pulse_width_us = THROTTLE_PULSE_WIDTH_HIGH;
- if (pulse_width_us < THROTTLE_PULSE_WIDTH_LOW)
- pulse_width_us = THROTTLE_PULSE_WIDTH_LOW;
- struct PWMOutputDriverState *state = self->state;
- *(state->outputs[channel] + PULSE_WIDTH_ADDR_OFFSET) = pulse_width_us;
- return 0;
-}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c
new file mode 100644
index 000000000..63d6ab525
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c
@@ -0,0 +1,40 @@
+#include "hw_impl_zybo.h"
+
+#define SYS_CLOCK_RATE 100000000 // ticks per second
+#define FREQUENCY 450
+#define PERIOD_WIDTH SYS_CLOCK_RATE/FREQUENCY
+#define PULSE_WIDTH_LOW SYS_CLOCK_RATE/1000 // 1 ms
+#define PULSE_WIDTH_HIGH SYS_CLOCK_RATE/500 // 2 ms
+#define PULSE_WIDTH_ADDR_OFFSET 1
+
+struct RCReceiverDriverState {
+ int *channels[6];
+};
+
+int zybo_rc_receiver_reset(struct RCReceiverDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(struct RCReceiverDriverState));
+ if (self->state == NULL) {
+ return -1;
+ }
+ }
+
+ // Save the addresses of the input PWM recorders
+ struct RCReceiverDriverState *state = self->state;
+ state->channels[0] = (int *) XPAR_PWM_RECORDER_0_BASEADDR;
+ state->channels[1] = (int *) XPAR_PWM_RECORDER_1_BASEADDR;
+ state->channels[2] = (int *) XPAR_PWM_RECORDER_2_BASEADDR;
+ state->channels[3] = (int *) XPAR_PWM_RECORDER_3_BASEADDR;
+ state->channels[4] = (int *) XPAR_PWM_RECORDER_4_BASEADDR;
+ state->channels[5] = (int *) XPAR_PWM_RECORDER_5_BASEADDR;
+ return 0;
+}
+
+int zybo_rc_receiver_read(struct RCReceiverDriver *self,
+ unsigned int channel,
+ float *magnitude) {
+ struct RCReceiverDriverState *state = self->state;
+ unsigned long pulse_width_ticks = (long) *((int *) state->channels[channel]);
+ *magnitude = (float) (pulse_width_ticks - PULSE_WIDTH_LOW) / (float) (PULSE_WIDTH_HIGH - PULSE_WIDTH_LOW);
+ return 0;
+}
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 2a4a32c80..5971d023b 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
@@ -159,18 +159,18 @@ int test_zybo_i2c_all() {
* 4) Set the RUN_TESTS macro in main.c
* 5) Uncomment only this test in main.c
* 6) Debug main.
- * 7) Observe the values of pwm_inputs in debugger chaning as you use the
+ * 7) Observe the values of pwm_inputs in debugger changing as you use the
* Spektrum RC controller.
*/
-int test_zybo_pwm_inputs() {
- struct PWMInputDriver pwm_inputs = create_zybo_pwm_inputs();
- pwm_inputs.reset(&pwm_inputs);
+int test_zybo_rc_receiver() {
+ struct RCReceiverDriver rc_receiver = create_zybo_rc_receiver();
+ rc_receiver.reset(&rc_receiver);
- unsigned long pwms[6];
+ float pwms[6];
while (1) {
int i;
for (i = 0; i < 6; i += 1) {
- pwm_inputs.read(&pwm_inputs, i, &pwms[i]);
+ rc_receiver.read(&rc_receiver, i, &pwms[i]);
}
continue;
}
@@ -188,22 +188,21 @@ int test_zybo_pwm_inputs() {
* 6) Run main.
* 7) Observe the PWM width of those PMOD pins changing with time
*/
-int test_zybo_pwm_outputs() {
- struct PWMOutputDriver pwm_outputs = create_zybo_pwm_outputs();
- pwm_outputs.reset(&pwm_outputs);
+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) {
- int j;
- for (j = 100000; j < 200000; j += 1) {
- int i;
+ for (j = 0; j < 1.0; j += 0.01) {
+ int i = 0;
for (i = 0; i < 4; i += 1) {
- pwm_outputs.write(&pwm_outputs, i, j);
- sys.sleep(&sys, 50);
+ motors.write(&motors, i, j);
+ sys.sleep(&sys, 50000);
}
}
- continue;
}
return 0;
}
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index ef3971feb..5892393e5 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -8,8 +8,8 @@
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_zybo_i2c();
- hardware->pwm_inputs = create_zybo_pwm_inputs();
- hardware->pwm_outputs = create_zybo_pwm_outputs();
+ hardware->rc_receiver = create_zybo_rc_receiver();
+ hardware->motors = create_zybo_motors();
hardware->uart = create_zybo_uart();
hardware->global_timer = create_zybo_global_timer();
hardware->axi_timer = create_zybo_axi_timer();
@@ -29,8 +29,8 @@ int main()
//test_zybo_i2c_imu();
//test_zybo_i2c_px4flow();
//test_zybo_i2c_all();
- //test_zybo_pwm_inputs();
- //test_zybo_pwm_outputs();
+ //test_zybo_rc_receiver();
+ test_zybo_motors();
//test_zybo_uart();
//test_zybo_axi_timer();
//test_zybo_uart();
--
GitLab