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
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 #include "ahrs_ardrone2.h"
31 #include "state.h"
33 #include "boards/ardrone/at_com.h"
34 #include "subsystems/electrical.h"
35 
36 #ifdef USE_GPS_ARDRONE2
38 #endif
39 
42 unsigned char buffer[4096]; //Packet buffer
43 
44 void ahrs_init(void) {
45  init_at_com();
46 
47  //Set navdata_demo to FALSE and flat trim the ar drone
48  at_com_send_config("general:navdata_demo", "FALSE");
50 
52 }
53 
54 void ahrs_align(void) {
55 
56 }
57 
58 void ahrs_propagate(void) {
59  //Recieve the main packet
61  navdata_t* main_packet = (navdata_t*) &buffer;
62 
63  //When this isn't a valid packet return
64  if(main_packet->header != NAVDATA_HEADER)
65  return;
66 
67  //Set the state
68  ahrs_impl.state = main_packet->ardrone_state;
69 
70  //Init the option
71  navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
72  bool_t full_read = FALSE;
73 
74  //The possible packets
75  navdata_demo_t* navdata_demo;
76  navdata_gps_t* navdata_gps;
77  navdata_phys_measures_t* navdata_phys_measures;
78 
79  //Read the navdata until packet is fully readed
80  while(!full_read && navdata_option->size > 0) {
81  //Check the tag for the right option
82  switch(navdata_option->tag) {
83  case 0: //NAVDATA_DEMO
84  navdata_demo = (navdata_demo_t*) navdata_option;
85 
86  //Set the AHRS state
87  ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
88  ahrs_impl.eulers.phi = navdata_demo->phi;
89  ahrs_impl.eulers.theta = navdata_demo->theta;
90  ahrs_impl.eulers.psi = navdata_demo->psi;
91  ahrs_impl.speed.x = navdata_demo->vx / 1000;
92  ahrs_impl.speed.y = navdata_demo->vy / 1000;
93  ahrs_impl.speed.z = navdata_demo->vz / 1000;
94  ahrs_impl.altitude = navdata_demo->altitude / 10;
96 
97  //Set the ned to body eulers
98  struct FloatEulers angles;
99  angles.theta = navdata_demo->theta/180000.*M_PI;
100  angles.psi = navdata_demo->psi/180000.*M_PI;
101  angles.phi = navdata_demo->phi/180000.*M_PI;
102  stateSetNedToBodyEulers_f(&angles);
103 
104  //Update the electrical supply
105  electrical.vsupply = navdata_demo->vbat_flying_percentage;
106  break;
107  case 3: //NAVDATA_PHYS_MEASURES
108  navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
109 
110  //Set the AHRS accel state
111  INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
112  break;
113 #ifdef USE_GPS_ARDRONE2
114  case 27: //NAVDATA_GPS
115  navdata_gps = (navdata_gps_t*) navdata_option;
116 
117  // Send the data to the gps parser
118  gps_ardrone2_parse(navdata_gps);
119  break;
120 #endif
121  case 0xFFFF: //CHECKSUM
122  //TODO: Check the checksum
123  full_read = TRUE;
124  break;
125  default:
126  //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
127  break;
128  }
129  navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
130  }
131 }
132 
133 void ahrs_update_accel(void) {
134 
135 }
136 
137 void ahrs_update_mag(void) {
138 
139 }
140 
141 void ahrs_update_gps(void) {
142 
143 }
144 
145 void ahrs_aligner_init(void) {
146 
147 }
148 
149 void ahrs_aligner_run(void) {
150 
151 }
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
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:8
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
float theta
in radians
unsigned char buffer[4096]
Definition: ahrs_ardrone2.c:42
euler angles
#define FALSE
Definition: imu_chimu.h:141
void at_com_send_ftrim(void)
Definition: at_com.c:159
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
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:54
float phi
in radians
#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
void at_com_recieve_navdata(unsigned char *buffer)
Definition: at_com.c:136
uint32_t header
Definition: at_com.h:101
struct FloatVect3 phys_accs
Definition: at_com.h:147
#define TRUE
Definition: imu_chimu.h:144
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
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:38
API to get/set the generic vehicle states.
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
struct AhrsAligner ahrs_aligner
Definition: ahrs_ardrone2.c:41
void init_at_com(void)
Definition: at_com.c:55
uint32_t battery
Definition: ahrs_ardrone2.h:45
void ahrs_propagate(void)
Propagation.
Definition: ahrs_ardrone2.c:58
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:44
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:40
void at_com_send_config(char *key, char *value)
Definition: at_com.c:149
void ahrs_aligner_init(void)
int32_t altitude
Definition: at_com.h:125
#define AHRS_RUNNING
Definition: ahrs.h:36
struct Electrical electrical