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