diff --git a/quad/executable.mk b/quad/executable.mk
index b7ddc1481916296dc569f90d378256b4a70c445b..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
@@ -31,7 +31,7 @@ clean:
$(TARGET): $(OBJECTS) | $(EXEDIR)
$(GCC) -g -o $(TARGET) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)
- cp $(TARGET) ./
+ 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 07bf7bea75ae923305ce078e7cab557ea2ac89c4..20c49fb33a75780419aa360794e8d3a757473766 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -12,7 +12,7 @@ bin_dir = script_dir + "/../../bin/"
Dir.chdir(bin_dir)
-Timeout::timeout(30) {
+Timeout::timeout(60) {
puts("Setting up...")
@@ -23,21 +23,25 @@ Timeout::timeout(30) {
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
- set_gear GEAR_OFF
- set_flap FLAP_ON
- `./virt-quad set rc_roll 0.498`
- `./virt-quad set rc_pitch 0.497`
- `./virt-quad set rc_yaw 0.498`
-
puts("Check that motors are off at startup")
check_motors_are_off "Motors weren't off at startup! How dangerous!"
@@ -80,12 +84,53 @@ Timeout::timeout(30) {
puts("Check that motors turn on")
set_throttle THROTTLE_MID
spinner = Thread.new { delay_spin_cursor(5) }
- motors = get_motor_averages(100, 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
- # motors.each { |value| assert_operator(value, :>, THROTTLE_EIGHTH) }
- motors_average = motors.reduce(:+).to_f / 4
- assert_operator(motors_average, :>, THROTTLE_EIGHTH)
+ 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")
set_gear GEAR_OFF
diff --git a/quad/scripts/tests/testing_library.rb b/quad/scripts/tests/testing_library.rb
index 9da10aef8f1d80cad84baf1ba22f1c0054c42154..6301d840d821f51356b1df742f7a1490fff98fb5 100644
--- a/quad/scripts/tests/testing_library.rb
+++ b/quad/scripts/tests/testing_library.rb
@@ -37,7 +37,11 @@ include Test::Unit::Assertions
# Utility functions
def check_motors_are_off(msg)
- motors = `./virt-quad get motors`.chomp.split("\n").map { |s| s.to_f }
+ 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)
}
@@ -63,7 +67,11 @@ end
def get_motor_averages(times, total_time)
motor_sums = [0.0, 0.0, 0.0, 0.0]
for i in 0..times
- motors = `./virt-quad get motors`.chomp.split("\n").map { |s| s.to_f }
+ 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
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 035085dfe81ad94f37648888e26bbe329c97d372..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/iic_utils.c
+++ /dev/null
@@ -1,329 +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);
-}
-
-void 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;
- }
-
- i2c->write(i2c, device_addr, buf, 2);
-}
-
-void 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;
- }
-
-
- i2c->write(i2c, device_addr, buf, 1);
- i2c->read(i2c, device_addr, recv_buffer, size);
-}
-
-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;
-
-}
-
-/**
- * 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);
- iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
-
- //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);
- 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);
- status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
- return status;
-}
-
-int iic0_px4flow_init(px4flow_t *of) {
- //Clear struct
- of->xPos = 0;
- of->yPos = 0;
- of->xVel = 0;
- of->yVel = 0;
-
- //Perform initial update
- return iic0_px4flow_update(of, 0.);
-}
-
-int iic0_px4flow_update(px4flow_t *of, double dt) {
- static double time = 0.;
-
- time += dt;
- if(time >= 10.) {
- time = 0.;
- }
-
- 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));
-
- //As per documentation, disregard frames with low quality, as they contain unreliable data
- if(i2cFrame.qual >= PX4FLOW_QUAL_MIN) {
- of->xVel = i2cFrame.flowCompX / 1000.;
- of->yVel = i2cFrame.flowCompY / 1000.;
-
- of->xPos += of->xVel * dt;
- of->yPos += of->yVel * dt;
- }
- }
-
- 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 637c9d7ab3b564be33b6ffa2922e3ba1e4ddaadd..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/iic_utils.h
+++ /dev/null
@@ -1,148 +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)
-
-///////////////////////////////////////////////////////////////////////////////
-// 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);
-
-void iic0_mpu9150_write(u8 register_addr, u8 data);
-void 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 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
-#define PX4FLOW_QUAL_MIN (100)
-
-
-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, double dt);
-
-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 6e85fa4a15c835be8e72ae7d47b2c39510f07cc6..5d43e5a1df71ecaf041e1341d01b6c260509591f 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -9,7 +9,6 @@
#include "communication.h"
#include "controllers.h"
#include "sensor.h"
-#include "iic_utils.h"
//#define BENCH_TEST
@@ -75,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;
@@ -95,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 f83cc8698f58c41a42ae255808f1c749b442bdbb..5195fb7b7876ba11d7653dc6667f5711822e8cf8 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -55,7 +55,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 b2f7592c88f667ca9ed9f897ea65dd4b22293ecc..d148f266f4a194fa35cb0d462851032c943fe2a0 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -10,74 +10,36 @@
#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;
-// }
+ struct IMUDriver *imu = &hardware_struct->imu;
+ struct LidarDriver *lidar = &hardware_struct->lidar;
+ struct OpticalFlowDriver *of = &hardware_struct->of;
- if(iic0_mpu9150_start() == -1) {
- return -1;
- }
+ if (imu->reset(imu)) {
+ return -1;
+ }
- // read sensor board and fill in gryo/accelerometer/magnetometer struct
- iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
+ 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;
+ // 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;
+ 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
+ struct IMUDriver *imu = &hardware_struct->imu;
+ struct LidarDriver *lidar = &hardware_struct->lidar;
+ struct OpticalFlowDriver *of = &hardware_struct->of;
+ int status = 0;
- int status = 0;
+ imu->read(imu, &raw_sensor_struct->gam);
- // the the sensor board and fill in the readings into the GAM struct
- iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
+ log_struct->gam = raw_sensor_struct->gam;
- 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;
-
- return 0;
+ 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 e2e40c1aac046af72fe499ef9b1f7f583bf27be5..c321d2221c5643cf7061919349e774dbffe49729 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,19 +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 xPos, yPos;
-} px4flow_t;
+ double distance;
+
+ double flowX, flowY;
+
+ int16_t quality;
+};
typedef struct PID_t {
float current_point; // Current value of the system
@@ -425,6 +425,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/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 296d30fbf58be8bd9c9db9a5cb7d97f5dea8e395..18119fb6b8ec03857a38e85e37fa8e3cdb409f99 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -25,6 +25,10 @@ struct VirtQuadIO {
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"
@@ -64,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();
@@ -72,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 aa2c1d5cee52eefc7e6c132534f1f6e5f14c754e..7bdd2d39be954f46505d6e439fac907bc514897c 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -3,9 +3,6 @@
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>
-#include "iic_utils.h"
-
-#define NUM_INPUTS 7
int unix_i2c_reset(struct I2CDriver *self) {
return 0;
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_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 85cf852c71681b83b6c7748273aa49cf4a659ee9..b4315760fe1a0e28b0ee44869169f89a029e56af 100644
--- a/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
+++ b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
@@ -8,6 +8,13 @@
extern struct VirtQuadIO *virt_quad_io;
int unix_rc_receiver_reset(struct RCReceiverDriver *self) {
+ // 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;
}
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
index d1c8c1e0abc7fc3d9725fb19a0556500fead17c6..105a8a5e68ae04d21e8001b9eb8c0b78a8629038 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -7,6 +7,8 @@
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;
@@ -24,39 +26,27 @@ 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, "qh")) != -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;
- case 'h':
- default: /* '?' */
- puts("from flags");
- usage(argv[0]);
- exit(EXIT_FAILURE);
}
- }
- if (argv[1] != NULL && strcmp(argv[1], "--help") == 0) {
- puts("from if");
- usage(argv[0]);
- exit(EXIT_FAILURE);
- }
- argc -= optind;
- argv += optind;
-
- // Decide if we are launching the quad or responding to a request
- // about a running quad
- if (argv[0] == NULL) {
// Prepare the shared memory for io. Clear memory and initialize.
- puts("Setting up...");
int *fd = &virt_quad_io_file_descriptor;
*fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR | O_TRUNC, 0666);
ftruncate(*fd, sizeof(struct VirtQuadIO));
@@ -67,20 +57,28 @@ int main(int argc, char *argv[]) {
pthread_mutex_init(&virt_quad_io->led_lock, &attr);
pthread_mutex_init(&virt_quad_io->motors_lock, &attr);
pthread_mutex_init(&virt_quad_io->rc_lock, &attr);
- puts("Finished setting up.");
}
else {
+ // 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);
- // Meet requests
- if (strcmp(argv[0], "get") == 0 && argv[1] != NULL) {
- handle_io_output(argv[1]);
+ 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[0], "set") == 0 && argv[1] != NULL && argv[2] != NULL) {
- handle_io_input(argv[1], 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;
}
@@ -98,17 +96,19 @@ int handle_io_output(const char *name) {
int ret = 0;
struct VirtQuadIO *io = virt_quad_io;
if (strcmp(name, "led") == 0) {
- pthread_mutex_lock(&io->led_lock);
- printf("%d\n", io->led);
- pthread_mutex_unlock(&io->led_lock);
+ print_shm_int(&io->led, &io->led_lock);
}
- else if (strcmp(name, "motors") == 0) {
- pthread_mutex_lock(&io->motors_lock);
- int i;
- for (i = 0; i < 4; i += 1) {
- printf("%f\n", io->motors[i]);
- }
- pthread_mutex_unlock(&io->motors_lock);
+ else 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.");
@@ -149,6 +149,33 @@ int handle_io_input(const char *name, const char *value_str) {
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;
@@ -163,6 +190,18 @@ void set_shm(float value, float *dest, pthread_mutex_t *lock) {
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:");
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 7748962c3f12530595b6664d64e3a712c9a9993e..02e6016775072254a104641f1313f598d1f99109 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,7 +1,35 @@
#include "hw_impl_zybo.h"
-#include "iic_utils.h"
#include "xiicps.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)
+
#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
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..7323a0e562a65b02eb9bfbc523941caaa9b83bc3
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -0,0 +1,221 @@
+#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
+
+void mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
+void mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
+
+void mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
+void 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] = {};
+
+ mpu9150_read(i2c, sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+
+ //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 0;
+}
+
+//////////////////////
+// Helper functions
+/////////////////////
+
+void 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;
+ }
+
+ i2c->write(i2c, device_addr, buf, 2);
+}
+
+void 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;
+ }
+
+
+ i2c->write(i2c, device_addr, buf, 1);
+ i2c->read(i2c, device_addr, recv_buffer, size);
+}
+
+
+void 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;
+}
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..9cd45df44b0289225b5ca9370c7e206450d176d5
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
@@ -0,0 +1,61 @@
+#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 status = 0;
+
+ // Device Reset & Wake up with default settings
+ status = lidarlite_write(i2c, 0x00, 0x00);
+ usleep(15000);
+
+ // Enable Free Running Mode and distance measurements with correction
+ status |= lidarlite_write(i2c, 0x11, 0xff);
+ status |= lidarlite_write(i2c, 0x00, 0x04);
+
+ return status;
+}
+
+int zybo_lidar_read(struct LidarDriver *self, lidar_t *lidar) {
+ struct I2CDriver *i2c = self->i2c;
+ u8 buf[2];
+ int status = 0;
+
+ // Read the sensor value
+ status = lidarlite_read(i2c, buf, 0x8f, 2);
+ float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
+ lidar->distance_m = distance_cm * 0.01;
+
+ return status;
+}
+
+////////////////////
+// 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 status = 0;
+
+ status |= i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
+ status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
+ return status;
+}
+
+// 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..aaef6eb3144244a00572a54539f7761370dd062c
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
@@ -0,0 +1,69 @@
+#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 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 = px4flow_read(i2c, buf, 0x00, sizeof(i2cFrame));
+
+ if (status == 0) {
+ of->xVel = i2cFrame.flowCompX / 1000.;
+ of->yVel = i2cFrame.flowCompY / 1000.;
+ of->quality = i2cFrame.qual;
+ of->distance = i2cFrame.groundDistance / 1000;
+ }
+
+ return status;
+}
+
+/////////////////////
+// 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 status = 0;
+
+ status |= i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
+ status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+ return status;
+}
+
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 5971d023b9f4026040217b8368e6ee5d7967ce4a..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, delay / 1000.);
- }
+ 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, delay / 1000.)) {
- 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 5892393e53137ad97d0c735c77d768c8f3a70d23..4b353eeb444eb1a773a3d7472472a49d201eb960 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();