Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
cam_track.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
23 #include "cam_track.h"
24 
25 #include "subsystems/ins.h"
26 #include "state.h"
27 
28 #if USE_HFF
30 #endif
31 
35 
37 
38 #define CAM_DATA_LEN (3*4)
39 #define CAM_START_1 0xFF
40 #define CAM_START_2 0xFE
41 #define CAM_END 0xF0
42 
43 #define UNINIT 0
44 #define GOT_START_1 1
45 #define GOT_START_2 2
46 #define GOT_LEN 3
47 #define GOT_DATA 4
48 #define GOT_END 5
49 
50 #include "messages.h"
52 
56 
57 void track_init(void) {
58  ins_ltp_initialised = TRUE; // ltp is initialized and centered on the target
59  ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground)
60 
63 
64 }
65 
66 #include <stdio.h>
67 void track_periodic_task(void) {
68  char cmd_msg[256];
69  uint8_t c = 0;
70 
71  cmd_msg[c++] = 'A';
72  cmd_msg[c++] = ' ';
73  struct FloatEulers* att = stateGetNedToBodyEulers_f();
74  float phi = att->phi;
75  if (phi > 0) cmd_msg[c++] = ' ';
76  else { cmd_msg[c++] = '-'; phi = -phi; }
77  cmd_msg[c++] = '0' + ((unsigned int) phi % 10);
78  cmd_msg[c++] = '0' + ((unsigned int) (10*phi) % 10);
79  cmd_msg[c++] = '0' + ((unsigned int) (100*phi) % 10);
80  cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10);
81  cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10);
82  cmd_msg[c++] = ' ';
83  float theta = att->theta;
84  if (theta > 0) cmd_msg[c++] = ' ';
85  else { cmd_msg[c++] = '-'; theta = -theta; }
86  cmd_msg[c++] = '0' + ((unsigned int) theta % 10);
87  cmd_msg[c++] = '0' + ((unsigned int) (10*theta) % 10);
88  cmd_msg[c++] = '0' + ((unsigned int) (100*theta) % 10);
89  cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10);
90  cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10);
91  cmd_msg[c++] = ' ';
92  float psi = att->psi;
93  if (psi > 0) cmd_msg[c++] = ' ';
94  else { cmd_msg[c++] = '-'; psi = -psi; }
95  cmd_msg[c++] = '0' + ((unsigned int) psi % 10);
96  cmd_msg[c++] = '0' + ((unsigned int) (10*psi) % 10);
97  cmd_msg[c++] = '0' + ((unsigned int) (100*psi) % 10);
98  cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10);
99  cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10);
100  cmd_msg[c++] = ' ';
101  float alt = stateGetPositionEnu_f()->z;
102  //alt = 0.40;
103  if (alt > 0) cmd_msg[c++] = ' ';
104  else { cmd_msg[c++] = '-'; alt = -alt; }
105  cmd_msg[c++] = '0' + ((unsigned int) (alt/10) % 10);
106  cmd_msg[c++] = '0' + ((unsigned int) alt % 10);
107  cmd_msg[c++] = '0' + ((unsigned int) (10*alt) % 10);
108  cmd_msg[c++] = '0' + ((unsigned int) (100*alt) % 10);
109  cmd_msg[c++] = '0' + ((unsigned int) (1000*alt) % 10);
110  cmd_msg[c++] = ' ';
111  cmd_msg[c++] = '\n';;
112 
113  int i;
114  for (i = 0; i < c; i++) {
115  CamUartSend1(cmd_msg[i]);
116  }
117  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,c,cmd_msg);
118 
119 }
120 
121 void track_event(void) {
122  if (!ins_ltp_initialised) {
124  ins.hf_realign = TRUE;
125  }
126 
127 #if USE_HFF
128  if (ins.hf_realign) {
129  ins.hf_realign = FALSE;
130  struct FloatVect2 pos, zero;
131  pos.x = -target_pos_ned.x;
132  pos.y = -target_pos_ned.y;
133  ins_realign_h(pos, zero);
134  }
135  const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 };
136  b2_hff_update_pos(-target_pos_ned, measurement_noise);
143 
145 #else
146  // store pos in ins
149  // compute speed from last pos
150  // TODO get delta T
151  // store last pos
153 
155 #endif
156 
158 }
159 
160 #define CAM_MAX_PAYLOAD 254
163 
164 void parse_cam_msg( void ) {
165  uint8_t* ptr;
166  // pos x
167  ptr = (uint8_t*)(&(target_pos_ned.x));
168  *ptr = cam_data_buf[0];
169  ptr++;
170  *ptr = cam_data_buf[1];
171  ptr++;
172  *ptr = cam_data_buf[2];
173  ptr++;
174  *ptr = cam_data_buf[3];
175  // pos y
176  ptr = (uint8_t*)(&(target_pos_ned.y));
177  *ptr = cam_data_buf[4];
178  ptr++;
179  *ptr = cam_data_buf[5];
180  ptr++;
181  *ptr = cam_data_buf[6];
182  ptr++;
183  *ptr = cam_data_buf[7];
184  // pos z
185  ptr = (uint8_t*)(&(target_pos_ned.z));
186  *ptr = cam_data_buf[8];
187  ptr++;
188  *ptr = cam_data_buf[9];
189  ptr++;
190  *ptr = cam_data_buf[10];
191  ptr++;
192  *ptr = cam_data_buf[11];
193 
194  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,12,cam_data_buf);
195 }
196 
198  char bla[1];
199  bla[1] = c;
200  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,1,bla);
201  switch (cam_status) {
202  case UNINIT:
203  if (c != CAM_START_1)
204  goto error;
205  cam_status++;
206  break;
207  case GOT_START_1:
208  if (c != CAM_START_2)
209  goto error;
210  cam_status++;
211  break;
212  case GOT_START_2:
213  cam_data_len = c;
215  goto error;
216  cam_data_idx = 0;
217  cam_status++;
218  break;
219  case GOT_LEN:
221  cam_data_idx++;
222  if (cam_data_idx >= cam_data_len)
223  cam_status++;
224  break;
225  case GOT_DATA:
226  if (c != CAM_END)
227  goto error;
229  goto restart;
230  break;
231  }
232  return;
233  error:
234  restart:
235  cam_status = UNINIT;
236  return;
237 }
238 
struct Ins ins
global INS state
Definition: ins.c:30
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:508
void track_init(void)
Definition: cam_track.c:57
#define CAM_START_2
Definition: cam_track.c:40
uint8_t cam_data_buf[CAM_MAX_PAYLOAD]
Definition: cam_track.c:161
#define CAM_DATA_LEN
Definition: cam_track.c:38
Horizontal filter (x,y) to estimate position and velocity.
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
volatile uint8_t cam_msg_received
Definition: cam_track.c:53
float xdot
Definition: hf_float.h:61
struct NedCoor_f ins_ltp_speed
Definition: ins_ardrone2.c:55
struct FloatVect3 target_accel_ned
Definition: cam_track.c:34
void track_event(void)
Definition: cam_track.c:121
float psi
in radians
#define ACCEL_BFP_OF_REAL(_af)
float y
in meters
float x
Definition: hf_float.h:59
float z
in meters
int32_t x
North.
#define GOT_DATA
Definition: cam_track.c:47
Integrated Navigation System interface.
float theta
in radians
float xdotdot
Definition: hf_float.h:62
struct NedCoor_f ins_ltp_accel
Definition: ins_ardrone2.c:54
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
euler angles
blob tracking with cmucam
bool_t hf_realign
realign horizontally if true
Definition: ins.h:46
bool_t ins_ltp_initialised
Definition: ins_ardrone2.c:57
#define FALSE
Definition: imu_chimu.h:141
struct FloatVect3 target_pos_ned
Definition: cam_track.c:32
float ydot
Definition: hf_float.h:65
float x
in meters
#define INS_NED_TO_STATE()
Definition: ins_int.h:73
#define CAM_END
Definition: cam_track.c:41
float phi
in radians
void parse_cam_buffer(uint8_t c)
Definition: cam_track.c:197
#define GOT_START_1
Definition: cam_track.c:44
uint16_t b2_hff_lost_counter
Definition: hf_float.c:217
int32_t y
East.
#define GOT_LEN
Definition: cam_track.c:46
struct FloatVect3 last_pos_ned
Definition: cam_track.c:36
struct HfilterFloat b2_hff_state
Definition: hf_float.c:86
#define TRUE
Definition: imu_chimu.h:144
#define CAM_MAX_PAYLOAD
Definition: cam_track.c:160
bool_t ins_update_on_agl
float ydotdot
Definition: hf_float.h:66
unsigned char uint8_t
Definition: types.h:14
float y
Definition: hf_float.h:63
API to get/set the generic vehicle states.
void parse_cam_msg(void)
Definition: cam_track.c:164
uint8_t cam_data_len
Definition: cam_track.c:55
void track_periodic_task(void)
Definition: cam_track.c:67
struct NedCoor_i ins_ltp_pos
Definition: ins_ardrone2.c:50
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:111
#define UNINIT
Definition: cam_track.c:43
uint8_t cam_status
Definition: cam_track.c:54
struct FloatVect3 target_speed_ned
Definition: cam_track.c:33
void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Definition: hf_float.c:646
static struct point c
Definition: discsurvey.c:39
#define CamUartSend1(c)
Definition: cam_track.h:54
#define GOT_START_2
Definition: cam_track.c:45
#define SPEED_BFP_OF_REAL(_af)
uint8_t cam_data_idx
Definition: cam_track.c:162
void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed)
INS horizontal realign.
Definition: ins_alt_float.c:78
#define POS_BFP_OF_REAL(_af)
#define CAM_START_1
Definition: cam_track.c:39