diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index 03b8550822b6d87b60bedcb44761f4d7673ed394..379b6e839ab935e07f374c203f9469d2b5d6bbcd 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/quad/src/quad_app/hw_iface.h
@@ -16,9 +16,9 @@
*/
// Forward declared types
-typedef struct gam gam_t;
-typedef struct lidar lidar_t;
-typedef struct px4flow px4flow_t;
+struct gam;
+struct lidar;
+struct px4flow;
struct RCReceiverDriver {
void *state;
@@ -74,20 +74,20 @@ struct I2CDriver {
struct IMUDriver {
struct I2CDriver *i2c;
- int (*reset)(struct IMUDriver *self);
- int (*read)(struct IMUDriver *self, gam_t *gam);
+ int (*reset)(struct IMUDriver *self, struct gam *gam);
+ int (*read)(struct IMUDriver *self, struct gam *gam);
};
struct LidarDriver {
struct I2CDriver *i2c;
- int (*reset)(struct LidarDriver *self);
- int (*read)(struct LidarDriver *self, lidar_t *lidar);
+ int (*reset)(struct LidarDriver *self, struct lidar *lidar);
+ int (*read)(struct LidarDriver *self, struct lidar *lidar);
};
struct OpticalFlowDriver {
struct I2CDriver *i2c;
- int (*reset)(struct OpticalFlowDriver *self);
- int (*read)(struct OpticalFlowDriver *self, px4flow_t *of);
+ int (*reset)(struct OpticalFlowDriver *self, struct px4flow *of);
+ int (*read)(struct OpticalFlowDriver *self, struct px4flow *of);
};
#endif
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 21f804de073ab189a739944072df920c62eaf7d9..7158c14fac93fbb57b551fc6e88eddefb2e6ce1b 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -15,7 +15,6 @@
#include "communication.h"
#include "computation_graph.h"
#include "graph_blocks.h"
-#include "iic_utils.h"
// Current index of the log array
static int arrayIndex = 0;
@@ -204,7 +203,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
int i;
// Comment header
safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
- safe_sprintf_cat(&buf, "# IIC failures: %d\n", iic_get_gam_timeout_failures());
+ safe_sprintf_cat(&buf, "# IIC failures: I'm not implemented yet\n");
// List Pid Constants
for (i = 0; i < ps->graph->n_nodes; ++i) {
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 1fd51a8c6ed1cf57822055659c123dc90df702b4..e57f8ba443705ebcf94b74365f15913be743a471 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -114,7 +114,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// Processing of loop timer at the end of the control loop
timer_end_loop(&(structs.log_struct));
- if (iic_get_consecutive_gam_timeout_failures() > 10) {
+ if (structs.raw_sensor_struct.gam.error.consErrorCount > 10) {
kill_motors(&(structs.hardware_struct.motors));
printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
break;
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index 7cd6a837542292274e4d3b54551ce00aa42cce75..079136a9f99abdc6beffa51106412c317a956b6c 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -16,13 +16,13 @@ int sensor_init(hardware_t *hardware_struct, raw_sensor_t * raw_sensor_struct, s
struct LidarDriver *lidar = &hardware_struct->lidar;
struct OpticalFlowDriver *of = &hardware_struct->of;
- if (imu->reset(imu)) {
+ if (imu->reset(imu, &raw_sensor_struct->gam)) {
return -1;
}
- if (lidar->reset(lidar)) {
+ if (lidar->reset(lidar, &raw_sensor_struct->lidar)) {
return -1;
}
- if (of->reset(of)) {
+ if (of->reset(of, &raw_sensor_struct->optical_flow)) {
return -1;
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 5f35184cc3f512fe6c6527ca8694c9530b4bdc0b..6e5653b95e4d64ff5c6ce375ab5d49e37fc29cc2 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -76,9 +76,14 @@ typedef struct {
float throttle;
} quadTrims_t;
+typedef struct SensorError {
+ uint32_t errorCount;
+ uint32_t consErrorCount;
+} SensorError_t;
+
//Gyro, accelerometer, and magnetometer data structure
//Used for reading an instance of the sensor data
-struct gam {
+typedef struct gam {
// GYRO
//Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
@@ -105,20 +110,26 @@ struct gam {
float mag_x; //Magnetic north: ~50 uT
float mag_y;
float mag_z;
-};
-struct lidar {
+ SensorError_t error;
+} gam_t;
+
+typedef struct lidar {
float distance_m; // distance in meters
-};
-struct px4flow {
+ SensorError_t error;
+} lidar_t;
+
+typedef struct px4flow {
double xVel, yVel;
double distance;
double flowX, flowY;
int16_t quality;
-};
+
+ SensorError_t error;
+} px4flow_t;
typedef struct PID_t {
float current_point; // Current value of the system
@@ -241,6 +252,8 @@ typedef struct raw_sensor {
gam_t gam;
+ lidar_t lidar;
+
// Structures to hold the current quad position & orientation
quadPosition_t currentQuadPosition;
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index 18119fb6b8ec03857a38e85e37fa8e3cdb409f99..b368f3fb50630324524ec9f50b1bb8daceb1287d 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -68,14 +68,14 @@ 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_imu_reset(struct IMUDriver *self, struct gam *gam);
+int unix_imu_read(struct IMUDriver *self, struct gam *gam);
-int unix_lidar_reset(struct LidarDriver *self);
-int unix_lidar_read(struct LidarDriver *self, lidar_t *lidar);
+int unix_lidar_reset(struct LidarDriver *self, struct lidar *lidar);
+int unix_lidar_read(struct LidarDriver *self, struct lidar *lidar);
-int unix_optical_flow_reset(struct OpticalFlowDriver *self);
-int unix_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of);
+int unix_optical_flow_reset(struct OpticalFlowDriver *self, struct px4flow *of);
+int unix_optical_flow_read(struct OpticalFlowDriver *self, struct px4flow *of);
struct UARTDriver create_unix_uart();
struct MotorDriver create_unix_motors();
diff --git a/quad/src/virt_quad/hw_impl_unix_imu.c b/quad/src/virt_quad/hw_impl_unix_imu.c
index 33639b424c349e0e0bc293fc924cdac7b0b519f9..300f0be6acc68622533463192f20b86fb7e7ee31 100644
--- a/quad/src/virt_quad/hw_impl_unix_imu.c
+++ b/quad/src/virt_quad/hw_impl_unix_imu.c
@@ -3,7 +3,7 @@
extern struct VirtQuadIO *virt_quad_io;
-int unix_imu_reset(struct IMUDriver *self) {
+int unix_imu_reset(struct IMUDriver *self, gam_t *gam) {
// Sensible defaults
virt_quad_io->gam.gyro_xVel_p = 0;
virt_quad_io->gam.gyro_yVel_q = 0;
diff --git a/quad/src/virt_quad/hw_impl_unix_lidar.c b/quad/src/virt_quad/hw_impl_unix_lidar.c
index fdf18894db7d6f35bc741e64e1a99cee079ff900..a7294aaaca5c7d5c9c680a71802bb6f109e08621 100644
--- a/quad/src/virt_quad/hw_impl_unix_lidar.c
+++ b/quad/src/virt_quad/hw_impl_unix_lidar.c
@@ -3,7 +3,7 @@
extern struct VirtQuadIO *virt_quad_io;
-int unix_lidar_reset(struct LidarDriver *self) {
+int unix_lidar_reset(struct LidarDriver *self, lidar_t *lidar) {
virt_quad_io->lidar.distance_m = 0;
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
index b2cf59f075104947513f85b7689ade116b65eb92..5763347c703bc6b211ac2a745c6e25fb17fa8f1d 100644
--- a/quad/src/virt_quad/hw_impl_unix_optical_flow.c
+++ b/quad/src/virt_quad/hw_impl_unix_optical_flow.c
@@ -3,7 +3,7 @@
extern struct VirtQuadIO *virt_quad_io;
-int unix_optical_flow_reset(struct OpticalFlowDriver *self) {
+int unix_optical_flow_reset(struct OpticalFlowDriver *self, px4flow_t *of) {
virt_quad_io->of.xVel = 0;
virt_quad_io->of.yVel = 0;
return 0;
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 fe2c11c67e5ca28bb601e9f04fa7b4f37b7251ce..ce579f1eaca33707e812906ccef0db764e082e76 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
@@ -17,6 +17,7 @@
#include "queue.h"
#include "platform.h"
#include "sleep.h"
+#include "type_def.h"
// Ideally, these defines would only be in the optical flow file, but
// i2c needs it for a certain hack
@@ -58,14 +59,14 @@ 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_imu_reset(struct IMUDriver *self, struct gam *gam);
+int zybo_imu_read(struct IMUDriver *self, struct gam *gam);
-int zybo_lidar_reset(struct LidarDriver *self);
-int zybo_lidar_read(struct LidarDriver *self, lidar_t *lidar);
+int zybo_lidar_reset(struct LidarDriver *self, struct lidar *lidar);
+int zybo_lidar_read(struct LidarDriver *self, struct lidar *lidar);
-int zybo_optical_flow_reset(struct OpticalFlowDriver *self);
-int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of);
+int zybo_optical_flow_reset(struct OpticalFlowDriver *self, struct px4flow *of);
+int zybo_optical_flow_read(struct OpticalFlowDriver *self, struct px4flow *of);
struct UARTDriver create_zybo_uart();
struct MotorDriver create_zybo_motors();
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
index d215cea0617e77e07116dab9b84afba0cd91655f..3bfc6c08da35e515fdff5b6ef916b890e987a63b 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -2,6 +2,8 @@
#include "hw_impl_zybo.h"
#include "type_def.h"
+#include <string.h>
+
#define MPU9150_DEVICE_ADDR 0b01101000
#define MPU9150_COMPASS_ADDR 0x0C
@@ -54,7 +56,10 @@ int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int s
int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
int mpu9150_read_gyro_accel(gam_t* gam);
-int zybo_imu_reset(struct IMUDriver *self) {
+int zybo_imu_reset(struct IMUDriver *self, gam_t *gam) {
+ // TODO: initialize gam
+ memset(gam, 0, sizeof(gam_t));
+
struct I2CDriver *i2c = self->i2c;
// Device Reset & Wake up
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
index f03ec059b4e90694e1218101158fb13325f2df11..cd49ca3657492b8b1c3b5b491b027fc648dbe404 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
@@ -1,6 +1,7 @@
#include "hw_iface.h"
#include "hw_impl_zybo.h"
#include "type_def.h"
+#include <string.h>
#define LIDARLITE_DEVICE_ADDR 0x62
#define LIDAR_OFFSET 0.02 // Distance from LiDAR sensor to ground, in meters
@@ -8,7 +9,10 @@
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) {
+int zybo_lidar_reset(struct LidarDriver *self, lidar_t *lidar) {
+ // initialize lidar
+ memset(lidar, 0, sizeof(lidar_t));
+
struct I2CDriver *i2c = self->i2c;
int error = 0;
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
index f6d463987ffb38160d48abf2b2181f414aac3fd3..f3ab4d9e66d993b53c6ddc7031bd17dec3450bde 100644
--- 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
@@ -1,13 +1,15 @@
#include "hw_iface.h"
#include "hw_impl_zybo.h"
#include <stdint.h>
+#include <string.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
+int zybo_optical_flow_reset(struct OpticalFlowDriver *self, px4flow_t *of) {
+ memset(of, 0, sizeof(px4flow_t));
+
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 1e74131ccf55d582139f9acbeb876fb217fdffe9..22bc4241ddf34e6709cd469502bc2217844af8e8 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
@@ -55,7 +55,7 @@ int test_zybo_i2c() {
i2c.reset(&i2c);
lidar_t lidar = { };
- if (ld.reset(&ld)) return 0;
+ if (ld.reset(&ld, &lidar)) return 0;
while (1) {
ld.read(&ld, &lidar);
}
@@ -67,8 +67,9 @@ int test_zybo_i2c_imu() {
struct IMUDriver imu = create_zybo_imu(&i2c);
i2c.reset(&i2c);
- if (imu.reset(&imu)) return 0;
- gam_t gam = { };
+ gam_t gam;
+ if (imu.reset(&imu, &gam)) return 0;
+
while (1) {
imu.read(&imu, &gam);
}
@@ -79,15 +80,14 @@ int test_zybo_i2c_px4flow() {
struct I2CDriver i2c = create_zybo_i2c();
struct OpticalFlowDriver ofd = create_zybo_optical_flow(&i2c);
i2c.reset(&i2c);
-
- if (ofd.reset(&ofd)) return 0;
-
px4flow_t of;
+ if (ofd.reset(&ofd, &of)) return 0;
+
for(;;) {
usleep(5000);
ofd.read(&ofd, &of);
- } struct IMUDriver imu = create_zybo_imu(&i2c);
+ }
return 0;
}
@@ -98,13 +98,14 @@ int test_zybo_i2c_all() {
struct OpticalFlowDriver ofd = create_zybo_optical_flow(&i2c);
i2c.reset(&i2c);
- 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;
+
+ if (ld.reset(&ld, &lidar)) return 0;
+ if (imu.reset(&imu, &gam)) return 0;
+ if (ofd.reset(&ofd, &of)) return 0;
- lidar_t lidar = { };
- px4flow_t of = { };
- gam_t gam = { };
int lidarErrors = 0;
int gamErrors = 0;