Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 //#include "subsystems/gps.h"
19 
20 #ifndef SENDER_ID
21 #define SENDER_ID 1
22 #endif
23 
25 #ifndef STEREOCAM_GPS_ID
26 #define STEREOCAM_GPS_ID ABI_BROADCAST
27 #endif
29 
30 
32 #ifndef USE_DEROTATION_OPTICFLOW
33 #define USE_DEROTATION_OPTICFLOW FALSE
34 #endif
35 #ifndef STATE_MEASURE_OPTICFLOW
36 #define STATE_MEASURE_OPTICFLOW TRUE
37 #endif
38 
39 static float prev_phi;
40 static float prev_theta;
41 
43 
44 void stereocam_to_state(float dphi, float dtheta);
45 
46 static void stereocam_gps_cb(uint8_t sender_id __attribute__((unused)),
47  uint32_t stamp __attribute__((unused)),
48  struct GpsState *gps_s)
49 {
53 
54 }
55 
57 {
58  //subscribe to GPS abi-messages for state measurements
59  AbiBindMsgGPS(STEREOCAM_GPS_ID, &gps_ev, stereocam_gps_cb);
60 
61 }
63 {
64 
65  if (stereocam_data.fresh) {
67  float phi = stateGetNedToBodyEulers_f()->phi;
68  float theta = stateGetNedToBodyEulers_f()->theta;
69  float dphi = phi - prev_phi;
70  float dtheta = theta - prev_theta;
71 
72  stereocam_to_state(dphi, dtheta);
73 
74  prev_theta = theta;
75  prev_phi = phi;
76  }
77 }
78 
79 void stereocam_to_state(float dphi, float dtheta)
80 {
81 
82  // Get info from stereocam data
83  float vel_hor = ((float)(stereocam_data.data[8]) - 127) / 100;
84  float vel_ver = ((float)(stereocam_data.data[9]) - 127) / 100;
85  float vel_x = 0;
86  float vel_y = 0;
87 
88 
89  // Calculate derotated velocity
90 #if USE_DEROTATION_OPTICFLOW
91  float agl_stereo = (float)(stereocam_data.data[4]) / 10;
92 
93 
94  float diff_flow_hor = dtheta * 128 / 1.04;
95  float diff_flow_ver = dphi * 96 / 0.785;
96 
97  float diff_vel_hor = diff_flow_hor * agl_stereo * 12 * 1.04 / 128;
98  float diff_vel_ver = diff_flow_ver * agl_stereo * 12 * 0.785 / 96;
99 
100  vel_x = - (vel_ver - diff_vel_ver);
101  vel_y = (vel_hor - diff_vel_hor);
102 #endif
103 
104  // Derotate velocity and transform from frame to body coordinates
105  vel_x = - (vel_ver);
106  vel_y = (vel_hor);
107 
108 
109 #if STATE_MEASURE_OPTICFLOW
110  // Calculate velocity in body fixed coordinates from opti-track and the state filter
111  struct NedCoor_f coordinates_speed_state;
112  coordinates_speed_state.x = stateGetSpeedNed_f()->x;
113  coordinates_speed_state.y = stateGetSpeedNed_f()->y;
114  coordinates_speed_state.z = stateGetSpeedNed_f()->z;
115 
116  struct NedCoor_f opti_state;
117  opti_state.x = (float)(gps_stereocam.ecef_vel.y) / 100;
118  opti_state.y = (float)(gps_stereocam.ecef_vel.x) / 100;
119  opti_state.z = -(float)(gps_stereocam.ecef_vel.z) / 100;
120 
121  struct FloatVect3 velocity_rot_state;
122  struct FloatVect3 velocity_rot_gps;
123 
124  float_rmat_vmult(&velocity_rot_state , stateGetNedToBodyRMat_f(), (struct FloatVect3 *)&coordinates_speed_state);
125  float_rmat_vmult(&velocity_rot_gps , stateGetNedToBodyRMat_f(), (struct FloatVect3 *)&opti_state);
126 
127  float vel_x_opti = -((float)(velocity_rot_gps.y));
128  float vel_y_opti = ((float)(velocity_rot_gps.x));
129 
130  // Calculate velocity error
131  float vel_x_error = vel_x_opti - vel_x;
132  float vel_y_error = vel_y_opti - vel_y;
133 
134 //TODO:: Check out why vel_x_opti is 10 x big as stereocamera's output
135  stereocam_data.data[8] = (uint8_t)((vel_x * 10) + 127); // dm/s
136  stereocam_data.data[9] = (uint8_t)((vel_y * 10) + 127); // dm/s
137  stereocam_data.data[19] = (uint8_t)((vel_x_opti) * 10 + 127); // dm/s
138  stereocam_data.data[20] = (uint8_t)((vel_y_opti) * 10 + 127); // dm/s
139  stereocam_data.data[21] = (uint8_t)((vel_x_error) * 10 + 127); // dm/s
140  stereocam_data.data[22] = (uint8_t)((vel_y_error) * 10 + 127); // dm/s
141  stereocam_data.data[23] = (uint8_t)((velocity_rot_state.x) * 10 + 127); // dm/s
142  stereocam_data.data[24] = (uint8_t)((velocity_rot_state.y) * 10 + 127); // dm/s
143 
144  //Send measurement values in same structure as stereocam message (make sure SEND_STEREO in stereocam.c is FALSE)
145  uint8_t frequency = 0;
146  DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), stereocam_data.len,
148 #endif
149 
150  //Send velocity estimate to state
151  //TODO:: Make variance dependable on line fit error, after new horizontal filter is made
152  uint32_t now_ts = get_sys_time_usec();
153 
154  if (!(abs(vel_y) > 0.5 || abs(vel_x) > 0.5) || abs(dphi) > 0.05 || abs(dtheta) > 0.05) {
155  AbiSendMsgVELOCITY_ESTIMATE(SENDER_ID, now_ts,
156  vel_x,
157  vel_y,
158  0.0f,
159  0.3f
160  );
161  }
162 
163 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
int32_t z
in centimeters
float y
in meters
static void stereocam_gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
float phi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t fresh
Definition: stereocam.h:35
int32_t y
in centimeters
#define SENDER_ID
Main include for ABI (AirBorneInterface).
static float prev_theta
uint8_t frequency
Definition: stereocam.c:47
void stereocam_to_state(float dphi, float dtheta)
void stereo_to_state_init(void)
#define STEREOCAM_GPS_ID
ABI binding for gps messages.
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
float z
in meters
struct EcefCoor_i ecef_vel
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1108
float theta
in radians
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
vector in North East Down coordinates Units: meters
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:885
data structure for GPS information
Definition: gps.h:67
unsigned long uint32_t
Definition: types.h:18
struct GpsStereoCam gps_stereocam
void stereo_to_state_periodic(void)
uint8array stereocam_data
Definition: stereocam.c:56
uint8_t len
Definition: stereocam.h:33
unsigned char uint8_t
Definition: types.h:14
int32_t x
in centimeters
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:72
static abi_event gps_ev
uint8_t * data
Definition: stereocam.h:34
static float prev_phi
float x
in meters