From 89189b158ed66cfec37847856618faa687703714 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <ucart@co3050-12>
Date: Fri, 14 Apr 2017 04:25:08 -0500
Subject: [PATCH] I2c watchdog timer.
---
quad/src/quad_app/control_algorithm.c | 6 ++--
quad/src/quad_app/iic_utils.c | 32 +++++++++++++++----
quad/src/quad_app/iic_utils.h | 9 ++++--
quad/src/quad_app/log_data.c | 2 ++
quad/src/quad_app/quad_app.c | 7 ++++
quad/src/quad_app/sensor_processing.c | 2 ++
quad/src/quad_app/timer.c | 2 +-
quad/src/quad_app/timer.h | 2 +-
.../real_quad/src/hw_impl_zybo_i2c.c | 15 +++++++++
quad/xsdk_workspace/real_quad/src/main.c | 6 ++++
10 files changed, 70 insertions(+), 13 deletions(-)
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index bd21410cd..692807713 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -17,7 +17,7 @@
#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
#define PX4FLOW_QUAL_MIN (100)
-#define OF_OFFSET_ANGLE (-0.62204) // -35.64 degrees
+#define OF_OFFSET_ANGLE (0.62204) // 35.64 degrees
#define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
#define PWM_DIFF_BOUNDS 20000
@@ -416,8 +416,8 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
// Compute VRPN blocks so they can be logged
- int outputs[1] = {ps->mixer};
- graph_compute_nodes(graph, outputs, 1);
+ int outputs[3] = {ps->mixer, ps->of_integ_x, ps->of_integ_y};
+ graph_compute_nodes(graph, outputs, 3);
// here for now so in case any flight command is not PID controlled, it will default to rc_command value:
//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index dffe2f231..5674ac93b 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -64,7 +64,7 @@ void iic0_mpu9150_stop(){
iic0_mpu9150_write(0x6B, 0b01000000);
}
-void iic0_mpu9150_write(u8 register_addr, u8 data){
+int iic0_mpu9150_write(u8 register_addr, u8 data){
u16 device_addr = MPU9150_DEVICE_ADDR;
u8 buf[] = {register_addr, data};
@@ -78,10 +78,10 @@ void iic0_mpu9150_write(u8 register_addr, u8 data){
device_addr = MPU9150_COMPASS_ADDR;
}
- i2c->write(i2c, device_addr, buf, 2);
+ return i2c->write(i2c, device_addr, buf, 2);
}
-void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
+int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
u16 device_addr = MPU9150_DEVICE_ADDR;
u8 buf[] = {register_addr};
@@ -96,8 +96,11 @@ void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
}
- i2c->write(i2c, device_addr, buf, 1);
- i2c->read(i2c, device_addr, recv_buffer, size);
+ int status = i2c->write(i2c, device_addr, buf, 1);
+ if (!status) {
+ status = i2c->read(i2c, device_addr, recv_buffer, size);
+ }
+ return status;
}
void iic0_mpu9150_calc_mag_sensitivity(){
@@ -156,6 +159,15 @@ void iic0_mpu9150_read_mag(gam_t* gam){
}
+static int iic_rx_timeout_failures;
+int iic_get_gam_timeout_failures() {
+ return iic_rx_timeout_failures;
+}
+static int iic_cons_timeout_failures;
+int iic_get_consecutive_gam_timeout_failures() {
+ return iic_cons_timeout_failures;
+}
+
/**
* Get Gyro Accel Mag (GAM) information
*/
@@ -170,7 +182,15 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
//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);
+ int status = iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+ if (status == IIC_RX_TIMEOUT_FAILURE) {
+ iic_rx_timeout_failures++;
+ iic_cons_timeout_failures++;
+ return status;
+ } else if (status) {
+ return status;
+ }
+ iic_cons_timeout_failures = 0; // Successful read
//Calculate accelerometer data
raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
diff --git a/quad/src/quad_app/iic_utils.h b/quad/src/quad_app/iic_utils.h
index 59ad8ed7c..c7f4bc0b8 100644
--- a/quad/src/quad_app/iic_utils.h
+++ b/quad/src/quad_app/iic_utils.h
@@ -46,6 +46,9 @@
#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)
+// Error code could be returned on an iic read if the watchdog timer triggers
+#define IIC_RX_TIMEOUT_FAILURE (-88)
+
///////////////////////////////////////////////////////////////////////////////
// MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
///////////////////////////////////////////////////////////////////////////////
@@ -98,8 +101,8 @@
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);
+int iic0_mpu9150_write(u8 register_addr, u8 data);
+int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
// Wake up the MPU for data collection
// Configure Gyro/Accel/Mag
@@ -112,6 +115,8 @@ void iic0_mpu9150_calc_mag_sensitivity();
void iic0_mpu9150_read_mag(gam_t* gam);
void iic0_mpu9150_read_gyro_accel(gam_t* gam);
+int iic_get_gam_timeout_failures();
+int iic_get_consecutive_gam_timeout_failures();
int iic0_mpu9150_read_gam(gam_t* gam);
////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 92c575f56..e83dce486 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -15,6 +15,7 @@
#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;
@@ -203,6 +204,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());
// 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 f83cc8698..a8a81a083 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -42,6 +42,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// Main control loop
while (1)
{
+ // Kill quad if kill switch or more than 10 GAM
int this_kill_condition = kill_condition(&(structs.user_input_struct));
// Processing of loop timer at the beginning of the control loop
@@ -112,6 +113,12 @@ 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) {
+ kill_motors(&(structs.hardware_struct.pwm_outputs));
+ printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+ break;
+ }
}
kill_motors(&(structs.hardware_struct.motors));
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 668afa23f..e6ba1660c 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -101,6 +101,8 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// Simply copy optical flow data
sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
+ sensor_struct->optical_flow.xVel = -sensor_struct->optical_flow.xVel;
+ sensor_struct->optical_flow.yVel = -sensor_struct->optical_flow.yVel;
return 0;
}
diff --git a/quad/src/quad_app/timer.c b/quad/src/quad_app/timer.c
index b7f68afbc..d6c9592f8 100644
--- a/quad/src/quad_app/timer.c
+++ b/quad/src/quad_app/timer.c
@@ -47,7 +47,7 @@ float get_last_loop_time() {
return (float)LOOP_TIME / 1000000;
}
-u32 timer_get_count() {
+u64 timer_get_count() {
u64 time;
return axi_timer->read(axi_timer, &time);
return time;
diff --git a/quad/src/quad_app/timer.h b/quad/src/quad_app/timer.h
index 8f3d12fb4..227175eaa 100644
--- a/quad/src/quad_app/timer.h
+++ b/quad/src/quad_app/timer.h
@@ -36,7 +36,7 @@ int timer_end_loop(log_t *log_struct);
// Returns the number of seconds the last loop took
float get_last_loop_time();
-u32 timer_get_count();
+u64 timer_get_count();
void timer_init_globals(struct TimerDriver *global_timer, struct TimerDriver *axi_timer);
#endif /* TIMER_H_ */
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 7748962c3..a2d662cb6 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,6 +1,7 @@
#include "hw_impl_zybo.h"
#include "iic_utils.h"
#include "xiicps.h"
+#include "timer.h"
#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
@@ -266,12 +267,26 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
ByteCount);
}
+/* <--- MicroCART additions (iic watchdog timer hack) ---> */
+ u32 iic_freq = XIicPs_GetSClk(InstancePtr);
+ // (1000000 * 9 / iic_freq) is the number of microseconds required to send 1 byte of data
+ // Using 5 times as an upper bound
+ u32 max_usec_per_byte = 5 * 1000000 * 9 / iic_freq;
+ u64 start_time = timer_get_count();
+/* <--- End hack ---> */
+
/*
* Pull the interrupt status register to find the errors.
*/
IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
while ((InstancePtr->RecvByteCount > 0) &&
((IntrStatusReg & Intrs) == 0) && !(IntrStatusReg & XIICPS_IXR_COMP_MASK)) {
+/* <--- MicroCART additions (iic watchdog timer hack) ---> */
+ u64 usec_passed = timer_get_count() - start_time;
+ if (usec_passed > max_usec_per_byte * (ByteCount - InstancePtr->RecvByteCount)) {
+ return IIC_RX_TIMEOUT_FAILURE;
+ }
+/* <--- End hack ---> */
StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
/*
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 5892393e5..6eeb6ce03 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -39,5 +39,11 @@ int main()
// Run the main quad application
quad_main(setup_hardware);
+
+ while(1) {
+ volatile int i = 0;
+ i++;
+ }
+
return 0;
}
--
GitLab