Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stereocam2state.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Kimberly McGuire
3  *
4  * This file is part of paparazzi
5  *
6  */
14 
15 #include "subsystems/abi.h"
17 
18 #ifndef STEREOCAM2STATE_SENDER_ID
19 #define STEREOCAM2STATE_SENDER_ID ABI_BROADCAST
20 #endif
21 
22 #ifndef STEREOCAM2STATE_RECEIVED_DATA_TYPE
23 #define STEREOCAM2STATE_RECEIVED_DATA_TYPE 0
24 #endif
25 
27 
28 void stereocam_to_state(void);
29 
31 {
32 
33 }
34 
36 {
37  if (stereocam_data.fresh) {
40  }
41 }
42 
44 {
45 
46  // Get info from stereocam data
47  // 0 = stereoboard's #define SEND_EDGEFLOW
48 #if STEREOCAM2STATE_RECEIVED_DATA_TYPE == 0
49  // opticflow
50  int16_t div_x = (int16_t)stereocam_data.data[0] << 8;
51  div_x |= (int16_t)stereocam_data.data[1];
52  int16_t flow_x = (int16_t)stereocam_data.data[2] << 8;
53  flow_x |= (int16_t)stereocam_data.data[3];
54  int16_t div_y = (int16_t)stereocam_data.data[4] << 8;
55  div_y |= (int16_t)stereocam_data.data[5];
56  int16_t flow_y = (int16_t)stereocam_data.data[6] << 8;
57  flow_y |= (int16_t)stereocam_data.data[7];
58 
59  float fps = (float)stereocam_data.data[9];
60  //int8_t agl = stereocam_data.data[8]; // in cm
61 
62  // velocity
63  int16_t vel_x_int = (int16_t)stereocam_data.data[10] << 8;
64  vel_x_int |= (int16_t)stereocam_data.data[11];
65  int16_t vel_y_int = (int16_t)stereocam_data.data[12] << 8;
66  vel_y_int |= (int16_t)stereocam_data.data[13];
67 
68  int16_t RES = 100;
69 
70  float vel_x = (float)vel_x_int / RES;
71  float vel_y = (float)vel_y_int / RES;
72 
73  // Derotate velocity and transform from frame to body coordinates
74  // TODO: send resolution directly from stereocam
75 
76  float vel_body_x = - vel_x;
77  float vel_body_y = vel_y;
78 
79  //Send velocity estimate to state
80  //TODO:: Make variance dependable on line fit error, after new horizontal filter is made
81  uint32_t now_ts = get_sys_time_usec();
82 
83  if (!(abs(vel_body_x) > 0.5 || abs(vel_body_x) > 0.5))
84  {
85  AbiSendMsgVELOCITY_ESTIMATE(STEREOCAM2STATE_SENDER_ID, now_ts,
86  vel_body_x,
87  vel_body_y,
88  0.0f,
89  0.3f
90  );
91  }
92 
93  // Reusing the OPTIC_FLOW_EST telemetry messages, with some values replaced by 0
94 
95  uint16_t dummy_uint16 = 0;
96  int16_t dummy_int16 = 0;
97  float dummy_float = 0;
98 
99  DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice, &fps, &dummy_uint16, &dummy_uint16, &flow_x, &flow_y, &dummy_int16, &dummy_int16,
100  &vel_x, &vel_y,&dummy_float, &dummy_float, &dummy_float);
101 
102 #endif
103 
104 }
unsigned short uint16_t
Definition: types.h:16
void stereocam_to_state(void)
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t fresh
Definition: stereocam.h:35
Main include for ABI (AirBorneInterface).
#define RES
Definition: detect_window.c:27
void stereo_to_state_init(void)
#define STEREOCAM2STATE_SENDER_ID
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
void stereo_to_state_periodic(void)
uint8array stereocam_data
Definition: stereocam.c:57
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
uint8_t * data
Definition: stereocam.h:34