Skip to content
Snippets Groups Projects
Commit ad6c29a4 authored by ucart's avatar ucart
Browse files

Merge branch 'master' of https://git.ece.iastate.edu/danc/MicroCART_17-18

parents 2fd2b423 2888cf88
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 245 additions and 589 deletions
quad/executable.mk
+ 2
− 2
View file @ ad6c29a4
......@@ -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
......
quad/scripts/tests/test_safety_checks.rb
+ 58
− 13
View file @ ad6c29a4
......@@ -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
......
quad/scripts/tests/testing_library.rb
+ 10
− 2
View file @ ad6c29a4
......@@ -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
......
quad/src/graph_blocks/node_mixer.c
+ 4
− 2
View file @ ad6c29a4
#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;
}
......
quad/src/quad_app/controllers.c
+ 0
− 1
View file @ ad6c29a4
......@@ -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"
......
quad/src/quad_app/hw_iface.h
+ 35
− 12
View file @ ad6c29a4
......@@ -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
quad/src/quad_app/iic_utils.c deleted 100644 → 0
+ 0
− 329
View file @ 2fd2b423
/**
* 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;
}
quad/src/quad_app/iic_utils.h deleted 100644 → 0
+ 0
− 148
View file @ 2fd2b423
/* 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*/
quad/src/quad_app/initialize_components.c
+ 4
− 3
View file @ ad6c29a4
......@@ -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);
......
quad/src/quad_app/initialize_components.h
+ 0
− 1
View file @ ad6c29a4
......@@ -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"
......
quad/src/quad_app/quad_app.c
+ 1
− 1
View file @ ad6c29a4
......@@ -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));
......
quad/src/quad_app/sensor.c
+ 20
− 58
View file @ ad6c29a4
......@@ -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;
}
quad/src/quad_app/sensor.h
+ 3
− 3
View file @ ad6c29a4
......@@ -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_ */
quad/src/quad_app/type_def.h
+ 14
− 11
View file @ ad6c29a4
......@@ -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;
/**
......
quad/src/virt_quad/hw_impl_unix.c
+ 24
− 0
View file @ ad6c29a4
......@@ -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;
}
quad/src/virt_quad/hw_impl_unix.h
+ 16
− 0
View file @ ad6c29a4
......@@ -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();
......
quad/src/virt_quad/hw_impl_unix_i2c.c
+ 0
− 3
View file @ ad6c29a4
......@@ -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;
......
quad/src/virt_quad/hw_impl_unix_imu.c 0 → 100644
+ 25
− 0
View file @ ad6c29a4
#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;
}
quad/src/virt_quad/hw_impl_unix_lidar.c 0 → 100644
+ 14
− 0
View file @ ad6c29a4
#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;
}
quad/src/virt_quad/hw_impl_unix_optical_flow.c 0 → 100644
+ 15
− 0
View file @ ad6c29a4
#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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment