Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pose_history.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Roland Meertens
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 #include <sys/time.h>
28 #include "mcu_periph/sys_time.h"
29 #include "state.h"
30 #ifdef __linux__
31 #include <pthread.h>
32 #endif
33 
34 #ifndef POSE_HISTORY_SIZE
35 #define POSE_HISTORY_SIZE 1024
36 #endif
37 
42 };
43 
45 
46 #ifdef __linux__
47 pthread_mutex_t pose_mutex;
48 #endif
49 
54 {
55 #ifdef __linux__
56  pthread_mutex_lock(&pose_mutex);
57 #endif
58  uint32_t index_history = 0;
59  uint32_t closestTimeDiff = timestamp - location_history.ring_data[0].timestamp;
60  uint32_t closestIndex = 0;
61 
62  // Seach for the timestamp closes to the timestamp argument of this function.
63  for (index_history = 0; index_history < location_history.ring_size; index_history++) {
64  uint32_t diff = timestamp - location_history.ring_data[index_history].timestamp;
65  if (diff < closestTimeDiff) {
66  closestIndex = index_history;
67  closestTimeDiff = diff;
68  }
69  }
70 
71  // Save the pose closest to the given timestamp and return this
72  struct pose_t closest_pose;
73  closest_pose.eulers = location_history.ring_data[closestIndex].eulers;
74  closest_pose.rates = location_history.ring_data[closestIndex].rates;
75 
76 #ifdef __linux__
77  pthread_mutex_unlock(&pose_mutex);
78 #endif
79  return closest_pose;
80 }
81 
85 void pose_init()
86 {
89 }
90 
91 
96 {
97  uint32_t now_ts = get_sys_time_usec();
98 #ifdef __linux__
99  pthread_mutex_lock(&pose_mutex);
100 #endif
101  struct pose_t *current_time_and_rotation = &location_history.ring_data[location_history.ring_index];
102  current_time_and_rotation->eulers = *stateGetNedToBodyEulers_f();
103  current_time_and_rotation->rates = *stateGetBodyRates_f();
104 
105  current_time_and_rotation->timestamp = now_ts;
106 
107  // increase index location history
109 
110 #ifdef __linux__
111  pthread_mutex_unlock(&pose_mutex);
112 #endif
113 }
114 
115 
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
void pose_init()
Initialises the pose history.
Definition: pose_history.c:85
void pose_periodic()
Records the pose history 512 times per second.
Definition: pose_history.c:95
struct pose_t ring_data[POSE_HISTORY_SIZE]
Definition: pose_history.c:41
#define POSE_HISTORY_SIZE
Definition: pose_history.c:35
struct rotation_history_ring_buffer_t location_history
Definition: pose_history.c:44
struct pose_t get_rotation_at_timestamp(uint32_t timestamp)
Given a pprz timestamp in used (obtained with get_sys_time_usec) we return the pose in FloatEulers cl...
Definition: pose_history.c:53
struct FloatRates rates
Definition: pose_history.h:34
uint32_t timestamp
Definition: pose_history.h:32
struct FloatEulers eulers
Definition: pose_history.h:33
API to get/set the generic vehicle states.
Architecture independent timing functions.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78