Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_ardrone2.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Freek van Tienen
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 
30 #ifdef ARDRONE2_DEBUG
31 # include <errno.h>
32 # include <stdio.h>
33 #endif
34 
35 #include "ahrs_ardrone2.h"
36 #include "state.h"
38 #include "boards/ardrone/at_com.h"
39 #include "subsystems/electrical.h"
40 
41 #ifdef USE_GPS_ARDRONE2
43 #endif
44 
47 unsigned char buffer[4096]; //Packet buffer
48 
49 #if PERIODIC_TELEMETRY
51 
52 static void send_ahrs_ad2(void) {
53  DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice,
59  &ahrs_impl.speed.x,
60  &ahrs_impl.speed.y,
61  &ahrs_impl.speed.z,
62  &ahrs_impl.accel.x,
63  &ahrs_impl.accel.y,
64  &ahrs_impl.accel.z,
67 }
68 #endif
69 
70 void ahrs_init(void) {
71  init_at_com();
72 
73  //Set navdata_demo to FALSE and flat trim the ar drone
74  at_com_send_config("general:navdata_demo", "FALSE");
76 
78 
79 #if PERIODIC_TELEMETRY
80  register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2);
81 #endif
82 }
83 
84 void ahrs_align(void) {
85 
86 }
87 
88 #ifdef ARDRONE2_DEBUG
89 static void dump(const void *_b, size_t s) {
90  const unsigned char *b = _b;
91  size_t n;
92 
93  for(n = 0; n < s; ++n) {
94  printf("%02x ", b[n]);
95  if (n%16 == 15)
96  printf("\n");
97  }
98  if (n%16 != 0)
99  printf("\n");
100 }
101 #endif
102 
103 void ahrs_propagate(void) {
104  int l;
105 
106  //Recieve the main packet
108  navdata_t* main_packet = (navdata_t*) &buffer;
109 
110 #ifdef ARDRONE2_DEBUG
111  if (l < 0)
112  printf("errno = %d\n", errno);
113 #endif
114 
115  //When this isn't a valid packet return
116  if(l < 0 || main_packet->header != NAVDATA_HEADER)
117  return;
118 
119 #ifdef ARDRONE2_DEBUG
120  printf("Read %d\n", l);
121  dump(buffer, l);
122 #endif
123 
124  //Set the state
125  ahrs_impl.state = main_packet->ardrone_state;
126 
127  //Init the option
128  navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
129  bool_t full_read = FALSE;
130 
131  //The possible packets
132  navdata_demo_t* navdata_demo;
133  navdata_gps_t* navdata_gps;
134  navdata_phys_measures_t* navdata_phys_measures;
135 
136  //Read the navdata until packet is fully readed
137  while(!full_read && navdata_option->size > 0) {
138 #ifdef ARDRONE2_DEBUG
139  printf ("tag = %d\n", navdata_option->tag);
140 #endif
141  //Check the tag for the right option
142  switch(navdata_option->tag) {
143  case 0: //NAVDATA_DEMO
144  navdata_demo = (navdata_demo_t*) navdata_option;
145 
146  //Set the AHRS state
147  ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
148  ahrs_impl.eulers.phi = navdata_demo->phi;
149  ahrs_impl.eulers.theta = navdata_demo->theta;
150  ahrs_impl.eulers.psi = navdata_demo->psi;
151  ahrs_impl.speed.x = navdata_demo->vx / 1000;
152  ahrs_impl.speed.y = navdata_demo->vy / 1000;
153  ahrs_impl.speed.z = navdata_demo->vz / 1000;
154  ahrs_impl.altitude = navdata_demo->altitude / 10;
155  ahrs_impl.battery = navdata_demo->vbat_flying_percentage;
156 
157  //Set the ned to body eulers
158  struct FloatEulers angles;
159  angles.theta = navdata_demo->theta/180000.*M_PI;
160  angles.psi = navdata_demo->psi/180000.*M_PI;
161  angles.phi = navdata_demo->phi/180000.*M_PI;
162  stateSetNedToBodyEulers_f(&angles);
163 
164  //Update the electrical supply
165  electrical.vsupply = navdata_demo->vbat_flying_percentage;
166  break;
167  case 3: //NAVDATA_PHYS_MEASURES
168  navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
169 
170  //Set the AHRS accel state
171  INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
172  break;
173 #ifdef USE_GPS_ARDRONE2
174  case 27: //NAVDATA_GPS
175 # ifdef ARDRONE2_DEBUG
176  dump(navdata_option, navdata_option->size);
177 # endif
178  navdata_gps = (navdata_gps_t*) navdata_option;
179 
180  // Send the data to the gps parser
181  gps_ardrone2_parse(navdata_gps);
182  break;
183 #endif
184  case 0xFFFF: //CHECKSUM
185  //TODO: Check the checksum
186  full_read = TRUE;
187  break;
188  default:
189 #ifdef ARDRONE2_DEBUG
190  printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
191 #endif
192  break;
193  }
194  navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
195  }
196 }
197 
198 void ahrs_aligner_init(void) {
199 }
200 
201 void ahrs_aligner_run(void) {
202 }
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
navdata_option_t options[1]
Definition: at_com.h:106
uint16_t tag
Definition: at_com.h:94
uint32_t ctrl_state
Definition: at_com.h:120
Periodic telemetry system header (includes downlink utility and generated code).
float theta
Definition: at_com.h:122
float psi
in radians
float y
in meters
void ahrs_aligner_run(void)
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
uint32_t vbat_flying_percentage
Definition: at_com.h:121
uint16_t size
Definition: at_com.h:95
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
struct NedCoor_f accel
Definition: ahrs_ardrone2.h:43
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
int32_t altitude
Definition: ahrs_ardrone2.h:44
int at_com_recieve_navdata(unsigned char *buffer)
Definition: at_com.c:136
float theta
in radians
unsigned char buffer[4096]
Definition: ahrs_ardrone2.c:47
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
#define FALSE
Definition: imu_chimu.h:141
void at_com_send_ftrim(void)
Definition: at_com.c:163
float x
in meters
struct FloatEulers eulers
Definition: ahrs_ardrone2.h:41
struct NedCoor_f speed
Definition: ahrs_ardrone2.h:42
Paparazzi floating point algebra.
AHRS implementation for ardrone2-sdk based on AT-commands.
void ahrs_align(void)
Aligns the AHRS.
Definition: ahrs_ardrone2.c:84
float phi
in radians
Interface for electrical status: supply voltage, current, battery status, etc.
#define NAVDATA_HEADER
Definition: at_com.h:32
uint32_t ardrone_state
Definition: at_com.h:102
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
float z
in meters
unsigned long uint32_t
Definition: types.h:18
struct FloatVect3 phys_accs
Definition: at_com.h:147
#define TRUE
Definition: imu_chimu.h:144
uint32_t state
Definition: ahrs_ardrone2.h:39
uint32_t control_state
Definition: ahrs_ardrone2.h:40
void gps_ardrone2_parse(navdata_gps_t *navdata_gps)
Definition: gps_ardrone2.c:42
API to get/set the generic vehicle states.
struct AhrsAligner ahrs_aligner
Definition: ahrs_ardrone2.c:46
struct Electrical electrical
void init_at_com(void)
Definition: at_com.c:55
uint32_t battery
Definition: ahrs_ardrone2.h:45
void ahrs_propagate(void)
Propagation.
Sending and receiving of AT-commands specified by the ardrone API.
ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2...
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_ardrone2.c:70
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:45
void at_com_send_config(char *key, char *value)
Definition: at_com.c:153
void ahrs_aligner_init(void)
int32_t altitude
Definition: at_com.h:125
#define AHRS_RUNNING
Definition: ahrs.h:36