Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
follow_me.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Roland
3  *
4  * This file is part of paparazzi
5  *
6  */
16 
17 #include "state.h"
18 #include "navigation.h"
20 #include "generated/flight_plan.h"
21 
22 #ifndef STEREOCAM_FOLLOW_ME_USE_OPTITRACK
23 #define STEREOCAM_FOLLOW_ME_USE_OPTITRACK FALSE
24 #endif
25 
26 #define HEADING_CHANGE_PER_MEASUREMENT 260
27 #define CENTER_IMAGE_HOR 65
28 #define MAXIMUM_ALTITUDE_FOLLOWING 3.0
29 #define MINIMUM_ALTITUDE_FOLLOWING 1.0
30 
31 float ref_pitch = 0.0;
32 float ref_roll = 0.0;
33 float selfie_alt = 1.0;
34 
35 static void changeRollYawPhase(int *phaseCounterArg, int *isRollPhaseArg, int *isYawPhaseArg)
36 {
37  static const int amountOfRollPhaseTime = 15;
38  static const int amountOfYawPhaseTime = 15;
39 
40  (*phaseCounterArg)++;
41 
42  if (*isRollPhaseArg) {
43  if (*phaseCounterArg > amountOfRollPhaseTime) {
44  *phaseCounterArg = 0;
45  *isRollPhaseArg = 0;
46  *isYawPhaseArg = 1;
47  }
48  } else {
49  if (*phaseCounterArg > amountOfYawPhaseTime) {
50  *phaseCounterArg = 0;
51  *isRollPhaseArg = 1;
52  *isYawPhaseArg = 0;
53  }
54  }
55 }
56 
57 static void increase_nav_heading(int32_t *heading, int32_t increment)
58 {
59  *heading = *heading + increment;
60 }
61 
62 void follow_me(uint8_t headingToFollow, uint8_t heightObject, uint8_t distanceToObject)
63 {
64  static int phaseCounter=0, isRollPhase=0, isYawPhase=0;
65  static float heightGain=0.3;
66 
67  // If we don't use GPS we alternate a phase where we roll and where we yaw.
68  // This way we don't drift sideways AND we don't lose the user out of our sight
69  changeRollYawPhase(&phaseCounter, &isRollPhase, &isYawPhase);
70 
71  // Change our heading if the user is starting to get out of sight.
72  float heading_change = 0.0;
73  int headingChangePerIt = HEADING_CHANGE_PER_MEASUREMENT;
74  if (abs(headingToFollow - CENTER_IMAGE_HOR) > 10) {
75  if (headingToFollow > CENTER_IMAGE_HOR) {
76  heading_change = 0.25;
77  if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) {
78  increase_nav_heading(&nav_heading, headingChangePerIt);
79  }
80  } else if (headingToFollow < CENTER_IMAGE_HOR) {
81  heading_change = -0.25;
82  if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) {
83  increase_nav_heading(&nav_heading, -1 * headingChangePerIt);
84  }
85  } else {
86  heading_change = 0.0;
87  }
88 
89  } else {
90  heading_change = 0.0;
91  }
92 
93  // If we have our roll phase we take the value of the change we need to have in heading and use it to go sideways
94  ref_roll = 0.0;
95  if (isRollPhase) {
96  ref_roll = 30 * heading_change;
97  }
98 
99 
100  // If we are in flight we want to adjust our height based on the place where we see our object
101  if (nav_is_in_flight()) {
102  if (heightObject > 50) {
103  selfie_alt -= heightGain;
104  } else if (heightObject < 20) {
105  selfie_alt += heightGain;
106  }
107  }
108 
109  // Bound the altitude to normal values
112  }
115  }
116 
117  // If using GPS we set the location of the waypoint to our desired altitude
118 #if STEREOCAM_FOLLOW_ME_USE_OPTITRACK
119  waypoint_set_alt(WP_STDBY, selfie_alt);
120 #endif
121 
122  // Set a pitch if the person we follow is too close
123  if (distanceToObject < 35) {
124  ref_pitch = 13.0;
125  } else if (distanceToObject < 60) {
126  ref_pitch = 5.0;
127  } else {
128  ref_pitch = -2.0;
129  }
130 }
#define MINIMUM_ALTITUDE_FOLLOWING
Definition: follow_me.c:29
Periodic telemetry system header (includes downlink utility and generated code).
#define CENTER_IMAGE_HOR
Definition: follow_me.c:27
float ref_pitch
Definition: follow_me.c:31
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference)
Definition: waypoints.c:169
static void changeRollYawPhase(int *phaseCounterArg, int *isRollPhaseArg, int *isYawPhaseArg)
Definition: follow_me.c:35
#define HEADING_CHANGE_PER_MEASUREMENT
Definition: follow_me.c:26
static float heading
Definition: ahrs_infrared.c:45
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:104
bool nav_is_in_flight(void)
Definition: navigation.c:415
static void increase_nav_heading(int32_t *heading, int32_t increment)
Definition: follow_me.c:57
signed long int32_t
Definition: types.h:19
#define MAXIMUM_ALTITUDE_FOLLOWING
Definition: follow_me.c:28
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void follow_me(uint8_t headingToFollow, uint8_t heightObject, uint8_t distanceToObject)
Definition: follow_me.c:62
float selfie_alt
Definition: follow_me.c:33
#define STEREOCAM_FOLLOW_ME_USE_OPTITRACK
Definition: follow_me.c:23
float ref_roll
Definition: follow_me.c:32