Added sonar status.

inc/i2c.h

@@ -39,7 +39,7 @@
 
 void i2c_init();
 void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t qual,
-        float ground_distance, float x_rate, float y_rate, float z_rate, int16_t gyro_temp);
+        float ground_distance, uint8_t sonar_status, float x_rate, float y_rate, float z_rate, int16_t gyro_temp);
 char i2c_get_ownaddress1();
 #endif /* I2C_H_ */
 

inc/i2c_frame.h

@@ -32,6 +32,13 @@
 #define I2C_FRAME_H_
 #include <inttypes.h>
 
+enum {
+    SONAR_STAT_NOMINAL = 0,
+    SONAR_STAT_UNINIT = 1,
+    SONAR_STAT_CLOSE = 2,
+    SONAR_STAT_FAR = 3,
+};
+
 // ***Take care***: struct alignment isn't necessarily what you expect,
 // so unaligned (i.e., single uint8_t values) should go at the end.
 // Otherwise nothing will work.
@@ -50,6 +57,7 @@ typedef  struct i2c_frame
     uint8_t gyro_range;
     uint8_t sonar_timestamp;
     int16_t ground_distance;
+    uint8_t sonar_status;
 } __attribute__((packed)) i2c_frame;
 
 #define I2C_FRAME_SIZE (sizeof(i2c_frame))
@@ -66,6 +74,7 @@ typedef struct i2c_integral_frame
     uint32_t integration_timespan;
     uint32_t sonar_timestamp;
     uint16_t ground_distance;
+    uint8_t sonar_status;
     int16_t gyro_temperature;
     uint8_t qual;
 } __attribute__((packed)) i2c_integral_frame;

inc/sonar.h

@@ -58,7 +58,7 @@ void sonar_trigger(void);
   *
   * @return true if valid measurement values were obtained, false else
   */
-bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw);
+uint8_t sonar_read(float* sonar_value_filtered, float* sonar_value_raw);
 
 /**
   * @brief Get the timestamp of the new sonar value when available to the main code

src/i2c.c

@@ -209,7 +209,8 @@ void I2C1_ER_IRQHandler(void) {
 
 void update_TX_buffer(float pixel_flow_x, float pixel_flow_y,
 		float flow_comp_m_x, float flow_comp_m_y, uint8_t qual,
-		float ground_distance, float gyro_x_rate, float gyro_y_rate,
+		float ground_distance, uint8_t sonar_status,
+		float gyro_x_rate, float gyro_y_rate,
 		float gyro_z_rate, int16_t gyro_temp) {
 	static uint16_t frame_count = 0;
 	int i;
@@ -225,6 +226,7 @@ void update_TX_buffer(float pixel_flow_x, float pixel_flow_y,
 	u.f.flow_comp_m_y = flow_comp_m_y * 1000;
 	u.f.qual = qual;
 	u.f.ground_distance = ground_distance * 1000;
+	u.f.sonar_status = sonar_status;
 
 	u.f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor();
 	u.f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor();

src/main.c

@@ -53,6 +53,7 @@
 #include "mt9v034.h"
 #include "gyro.h"
 #include "i2c.h"
+#include "i2c_frame.h"
 #include "usart.h"
 #include "sonar.h"
 #include "communication.h"
@@ -261,7 +262,7 @@ int main(void)
 	/* sonar config*/
 	float sonar_distance_filtered = 0.0f; // distance in meter
 	float sonar_distance_raw = 0.0f; // distance in meter
-	bool distance_valid = false;
+	uint8_t sonar_status = SONAR_STAT_UNINIT;
 	sonar_config();
 
 	/* reset/start timers */
@@ -365,13 +366,7 @@ int main(void)
 		const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
 
 		/* get sonar data */
-		distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
-
-		/* reset to zero for invalid distances */
-		if (!distance_valid) {
-			sonar_distance_filtered = 0.0f;
-			sonar_distance_raw = 0.0f;
-		}
+		sonar_status = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
 
 		/* compute optical flow */
 		if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
@@ -391,7 +386,7 @@ int main(void)
 			float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
 
 			/* integrate velocity and output values only if distance is valid */
-			if (distance_valid)
+			if (sonar_status == SONAR_STAT_NOMINAL)
 			{
 				/* calc velocity (negative of flow values scaled with distance) */
 				float new_velocity_x = - flow_compx * sonar_distance_filtered;
@@ -464,13 +459,17 @@ int main(void)
 			//update I2C transmitbuffer
 			if(valid_frame_count>0)
 			{
-				update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
-						ground_distance, x_rate, y_rate, z_rate, gyro_temp);
+				update_TX_buffer(pixel_flow_x, pixel_flow_y,
+					velocity_x_sum/valid_frame_count,
+					velocity_y_sum/valid_frame_count, qual,
+					ground_distance, sonar_status,
+					x_rate, y_rate, z_rate, gyro_temp);
 			}
 			else
 			{
 				update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual,
-						ground_distance, x_rate, y_rate, z_rate, gyro_temp);
+					ground_distance, sonar_status,
+					x_rate, y_rate, z_rate, gyro_temp);
 			}
 
             //serial mavlink  + usb mavlink output throttled

src/sonar.c

@@ -49,6 +49,7 @@
 #include "settings.h"
 #include "sonar.h"
 #include "sonar_mode_filter.h"
+#include "i2c_frame.h"
 
 #define SONAR_SCALE	1000.0f
 #define SONAR_MIN	0.12f		/** 0.12m sonar minimum distance */
@@ -79,7 +80,7 @@ float v_post = 0.0f; // m/s
 float sonar_raw = 0.0f;  // m
 
 float sonar_mode = 0.0f;
-float sonar_valid = false;				/**< the mode of all sonar measurements */
+float sonar_status = SONAR_STAT_UNINIT;				/**< the mode of all sonar measurements */
 
 /**
   * @brief  Triggers the sonar to measure the next value
@@ -125,8 +126,12 @@ void UART4_IRQHandler(void)
 				int temp = atoi(data_buffer);
 
 				/* use real-world maximum ranges to cut off pure noise */
-				if ((temp > SONAR_MIN*SONAR_SCALE) && (temp < SONAR_MAX*SONAR_SCALE))
-				{
+				if (temp < SONAR_MIN*SONAR_SCALE) {
+					sonar_status = SONAR_STAT_CLOSE;
+				} else if (temp > SONAR_MAX*SONAR_SCALE) {
+					sonar_status = SONAR_STAT_FAR;
+				} else {
+					sonar_status = SONAR_STAT_NOMINAL;
 					/* it is in normal sensor range, take it */
 					last_measure_time = measure_time;
 					measure_time = get_boot_time_us();
@@ -136,9 +141,6 @@ void UART4_IRQHandler(void)
 					valid_data = temp;
 					sonar_mode = insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);
 					new_value = 1;
-					sonar_valid = true;
-				} else {
-					sonar_valid = false;
 				}
 			}
 
@@ -179,7 +181,7 @@ void sonar_filter()
   * @param  sonar_value_filtered Filtered return value
   * @param  sonar_value_raw Raw return value
   */
-bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw)
+uint8_t sonar_read(float* sonar_value_filtered, float* sonar_value_raw)
 {
 	/* getting new data with only around 10Hz */
 	if (new_value) {
@@ -189,14 +191,16 @@ bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw)
 	}
 
 	/* catch post-filter out of band values */
-	if (x_post < SONAR_MIN || x_post > SONAR_MAX) {
-		sonar_valid = false;
+	if (x_post < SONAR_MIN) {
+		sonar_status = SONAR_STAT_CLOSE;
+	} else if (x_post > SONAR_MAX) {
+		sonar_status = SONAR_STAT_FAR;
 	}
 
 	*sonar_value_filtered = x_post;
 	*sonar_value_raw = sonar_raw;
 
-	return sonar_valid;
+	return sonar_status;
 }
 
 /**