Klt flow

Makefile

@@ -10,7 +10,7 @@ JTAGCONFIG 	?= olimex-jtag-tiny.cfg
 #JTAGCONFIG	?= olimex-arm-usb-tiny-h.cfg
 
 MAVLINKBASEDIR = mavlink/include/mavlink/v1.0
-MAVLINKDIR = mavlink/include/mavlink/v1.0/pixhawk
+MAVLINKDIR = mavlink/include/mavlink/v1.0/common
 MAVLINKUSERDIR = mavlink/include/mavlink/v1.0/user
 
 AS =		arm-none-eabi-as
@@ -57,7 +57,7 @@ SRCS += 	lib/STM32F4xx_StdPeriph_Driver/src/misc.c \
 .PHONY: clean upload-usb
 
 CFLAGS		 = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 \
-			-O1 -g \
+			-O3 \
 			-std=gnu99 \
 			-Wall \
 			-MD \

inc/communication.h

@@ -48,7 +48,6 @@ void communication_receive_forward(void);
 void communication_receive_usb(void);
 void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg);
 
-void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t * ch, uint16_t length);
 mavlink_status_t* mavlink_get_channel_status(uint8_t channel);
 mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel);
 

inc/flow.h

@@ -42,4 +42,10 @@
 uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate,
 		float *histflowx, float *histflowy);
 
+/**
+ * @brief Computes pixel flow from image1 to image2 with KLT method
+ */
+uint8_t compute_klt(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate,
+    float *pixel_flow_x, float *pixel_flow_y);
+
 #endif /* FLOW_H_ */

inc/gyro.h

@@ -78,6 +78,8 @@
 #define ADDR_REFERENCE				0x25
 #define ADDR_TEMPERATURE			0x26
 
+#define L3GD20_TEMP_OFFSET_CELSIUS  40
+
 #define ADDR_STATUS_REG				0x27
 #define STATUS_ZYXOR				(1<<7)
 #define SATAUS_ZOR					(1<<6)
@@ -145,7 +147,7 @@ void gyro_config(void);
 /**
  * @brief Read out newest gyro value
  */
-void gyro_read(float* x_rate, float* y_rate, float* z_rate);
+void gyro_read(float* x_rate, float* y_rate, float* z_rate, int16_t* gyro_temp);
 
 /* Low layer functions */
 void spi_config(void);

inc/i2c.h

@@ -38,8 +38,8 @@
  */
 
 void i2c_init();
-void update_TX_buffer(float pixel_flow_x_sum, float pixel_flow_y_sum, float flow_comp_m_x, float flow_comp_m_y, uint16_t qual,
-        float ground_distance, float x_rate, float y_rate, float z_rate);
+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);
 char i2c_get_ownaddress1();
 #endif /* I2C_H_ */
 

inc/i2c_frame.h

@@ -32,8 +32,11 @@
 #define I2C_FRAME_H_
 #include <inttypes.h>
 
-#define I2C_FRAME_SIZE (sizeof(i2c_frame))
-typedef struct i2c_frame
+// ***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.
+
+typedef  struct i2c_frame
 {
     uint16_t frame_count;
     int16_t pixel_flow_x_sum;
@@ -47,6 +50,26 @@ typedef struct i2c_frame
     uint8_t gyro_range;
     uint8_t sonar_timestamp;
     int16_t ground_distance;
-} i2c_frame;
+} __attribute__((packed)) i2c_frame;
+
+#define I2C_FRAME_SIZE (sizeof(i2c_frame))
+
+
+typedef struct i2c_integral_frame
+{
+    uint16_t frame_count_since_last_readout;
+    int16_t pixel_flow_x_integral;
+    int16_t pixel_flow_y_integral;
+    int16_t gyro_x_rate_integral;
+    int16_t gyro_y_rate_integral;
+    int16_t gyro_z_rate_integral;
+    uint32_t integration_timespan;
+    uint32_t sonar_timestamp;
+    uint16_t ground_distance;
+    int16_t gyro_temperature;
+    uint8_t qual;
+} __attribute__((packed)) i2c_integral_frame;
+
+#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
 
 #endif /* I2C_FRAME_H_ */

inc/main.h

@@ -37,6 +37,8 @@
 extern uint32_t get_time_between_images(void);
 
 void timer_update(void);
+void timer_update_ms(void);
 uint32_t get_boot_time_ms(void);
+uint32_t get_boot_time_us(void);
 
 #endif /* __PX4_FLOWBOARD_H */

inc/mavlink_bridge_header.h

@@ -49,7 +49,7 @@
 extern mavlink_system_t mavlink_system;
 
 /* defined in communication.c */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t * ch, uint16_t length);
+extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t * ch, uint16_t length);
 
 mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
 mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);

inc/settings.h

@@ -138,6 +138,7 @@ enum
 	PARAM_BOTTOM_FLOW_GYRO_COMPENSATION,
 	PARAM_BOTTOM_FLOW_LP_FILTERED,
 	PARAM_BOTTOM_FLOW_WEIGHT_NEW,
+	PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR,
 
 	PARAM_SENSOR_POSITION,
 	DEBUG_VARIABLE,

inc/usart.h

@@ -59,7 +59,7 @@ uint8_t usart3_rx_ringbuffer_pop(void);
 /**
   * @brief  Push one byte to ringbuffer of USART3
   */
-uint8_t usart3_tx_ringbuffer_push(uint8_t* ch, uint8_t len);
+uint8_t usart3_tx_ringbuffer_push(const uint8_t* ch, uint8_t len);
 
 /**
   * @brief  Check character availability USART2