From cd80ff904a0382eb0d8e30d4ee2a5ed5793938ec Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Wed, 19 Apr 2017 18:37:01 -0500
Subject: [PATCH] quad: wip: setup error counters in quad app
---
quad/src/quad_app/hw_iface.h | 18 ++++++-------
quad/src/quad_app/log_data.c | 3 +--
quad/src/quad_app/quad_app.c | 2 +-
quad/src/quad_app/sensor.c | 6 ++---
quad/src/quad_app/type_def.h | 25 ++++++++++++-----
quad/src/virt_quad/hw_impl_unix.h | 12 ++++-----
quad/src/virt_quad/hw_impl_unix_imu.c | 2 +-
quad/src/virt_quad/hw_impl_unix_lidar.c | 2 +-
.../src/virt_quad/hw_impl_unix_optical_flow.c | 2 +-
.../real_quad/src/hw_impl_zybo.h | 13 ++++-----
.../real_quad/src/hw_impl_zybo_imu.c | 7 ++++-
.../real_quad/src/hw_impl_zybo_lidar.c | 6 ++++-
.../real_quad/src/hw_impl_zybo_optical_flow.c | 6 +++--
.../real_quad/src/hw_impl_zybo_tests.c | 27 ++++++++++---------
14 files changed, 78 insertions(+), 53 deletions(-)
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index 03b855082..379b6e839 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 21f804de0..7158c14fa 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 1fd51a8c6..e57f8ba44 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 7cd6a8375..079136a9f 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 5f35184cc..6e5653b95 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 18119fb6b..b368f3fb5 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 33639b424..300f0be6a 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 fdf18894d..a7294aaac 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 b2cf59f07..5763347c7 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 fe2c11c67..ce579f1ea 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 d215cea06..3bfc6c08d 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 f03ec059b..cd49ca365 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 f6d463987..f3ab4d9e6 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 1e74131cc..22bc4241d 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;
--
GitLab