Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
autopilot.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Gautier Hattenberger
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 
29 #include <stdint.h>
31 
32 #include "state.h"
34 
35 #ifdef POWER_SWITCH_GPIO
36 #include "mcu_periph/gpio.h"
37 #endif
38 
39 #include "pprz_version.h"
40 
44 
45 bool launch;
46 
49 
51 
54 float energy;
55 
56 bool gps_lost;
57 
59 
60 #if PERIODIC_TELEMETRY
62 #include "generated/settings.h"
63 
64 void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
65 {
66  static uint32_t ap_version = PPRZ_VERSION_INT;
67  static char *ver_desc = PPRZ_VERSION_DESC;
68  pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
69 }
70 
71 static void send_alive(struct transport_tx *trans, struct link_device *dev)
72 {
73  pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
74 }
75 
76 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
77 #include "rc_settings.h"
78 static void send_rc_settings(struct transport_tx *trans, struct link_device *dev)
79 {
80  if (!RcSettingsOff()) {
81  pprz_msg_send_SETTINGS(trans, dev, AC_ID, &slider_1_val, &slider_2_val);
82  }
83 }
84 #else
86 #endif
87 
88 static void send_mode(struct transport_tx *trans, struct link_device *dev)
89 {
90  pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID,
91  &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
92 }
93 
94 static void send_attitude(struct transport_tx *trans, struct link_device *dev)
95 {
96  struct FloatEulers *att = stateGetNedToBodyEulers_f();
97  pprz_msg_send_ATTITUDE(trans, dev, AC_ID,
98  &(att->phi), &(att->psi), &(att->theta));
99 };
100 
101 static void send_estimator(struct transport_tx *trans, struct link_device *dev)
102 {
103  pprz_msg_send_ESTIMATOR(trans, dev, AC_ID,
104  &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
105 }
106 
107 static void send_bat(struct transport_tx *trans, struct link_device *dev)
108 {
109  int16_t amps = (int16_t)(current / 10);
110  int16_t e = energy;
111  // prevent overflow
112  if (fabs(energy) >= INT16_MAX) {
113  e = INT16_MAX;
114  }
115  pprz_msg_send_BAT(trans, dev, AC_ID,
116  &v_ctl_throttle_slewed, &vsupply, &amps,
118  &block_time, &stage_time, &e);
119 }
120 
121 static void send_energy(struct transport_tx *trans, struct link_device *dev)
122 {
123  uint16_t e = energy;
124  if (fabs(energy) >= INT16_MAX) {
125  e = INT16_MAX;
126  }
127  float vsup = ((float)vsupply) / 10.0f;
128  float curs = ((float)current) / 1000.0f;
129  float power = vsup * curs;
130  pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power);
131 }
132 
133 static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
134 {
135  PeriodicSendDlValue(trans, dev);
136 }
137 
138 // FIXME not the best place
140 #include CTRL_TYPE_H
141 static void send_desired(struct transport_tx *trans, struct link_device *dev)
142 {
143 #ifndef USE_AIRSPEED
144  float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
145 #endif
146  pprz_msg_send_DESIRED(trans, dev, AC_ID,
149  &v_ctl_auto_airspeed_setpoint);
150 }
151 
152 static void send_airspeed(struct transport_tx *trans __attribute__((unused)),
153  struct link_device *dev __attribute__((unused)))
154 {
155  float airspeed = stateGetAirspeed_f();
156 #if USE_AIRSPEED
157  pprz_msg_send_AIRSPEED(trans, dev, AC_ID,
158  &airspeed, &v_ctl_auto_airspeed_setpoint,
160 #else
161  float zero = 0;
162  pprz_msg_send_AIRSPEED(trans, dev, AC_ID, &airspeed, &zero, &zero, &zero);
163 #endif
164 }
165 #endif /* PERIODIC_TELEMETRY */
166 
168 {
169  // use default telemetry here
170 #if DOWNLINK
171  send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
172 #endif
173 }
174 
175 void autopilot_init(void)
176 {
178  kill_throttle = false;
179  launch = false;
181 
183 
184  gps_lost = false;
185 
186  power_switch = false;
187 #ifdef POWER_SWITCH_GPIO
190 #endif
191 
192 #if PERIODIC_TELEMETRY
193  /* register some periodic message */
194  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
196  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PPRZ_MODE, send_mode);
204 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
205  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC_SETTINGS, send_rc_settings);
206 #endif
207 #endif
208 }
209 
uint16_t block_time
unsigned short uint16_t
Definition: types.h:16
uint16_t vsupply
Supply voltage in deciVolt.
Definition: autopilot.c:52
float phi
in radians
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
#define POWER_SWITCH_GPIO
Definition: apogee_1.0.h:95
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1125
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:175
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:103
#define LATERAL_MODE_MANUAL
Definition: autopilot.h:74
Variable setting though the radio control.
Periodic telemetry system header (includes downlink utility and generated code).
bool gps_lost
Definition: autopilot.c:56
#define RcSettingsOff()
Definition: rc_settings.h:70
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:48
float h_ctl_course_setpoint
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:125
uint8_t pprz_mode
Definition: autopilot.c:41
uint8_t lateral_mode
Definition: autopilot.c:50
Some architecture independent helper functions for GPIOs.
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:98
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1389
float psi
in radians
static void send_energy(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:121
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:52
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:133
static void send_estimator(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:101
int32_t current
Supply current in milliAmpere.
Definition: autopilot.c:53
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:94
euler angles
Fixed wing horizontal control.
bool kill_throttle
Definition: autopilot.c:42
float theta
in radians
bool power_switch
Power switch control.
Definition: autopilot.c:58
unsigned long uint32_t
Definition: types.h:18
float h_ctl_pitch_loop_setpoint
signed short int16_t
Definition: types.h:17
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
float slider_1_val
Definition: rc_settings.c:35
static void send_bat(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:107
bool launch
Definition: autopilot.c:45
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
static void send_airspeed(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:152
uint16_t stage_time
In s.
void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:64
signed long int32_t
Definition: types.h:19
uint8_t rc_settings_mode
Definition: autopilot.c:85
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void autopilot_send_mode(void)
Send mode over telemetry.
Definition: autopilot.c:167
static void send_mode(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:88
float energy
Energy consumption (mAh) This is the ap copy of the measurement from fbw.
Definition: autopilot.c:54
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:686
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float slider_2_val
Definition: rc_settings.c:35
float h_ctl_roll_setpoint
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:905
uint8_t mcu1_status
Definition: autopilot.c:43
static void send_desired(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:141
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:123
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static void send_alive(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:71
Fixedwing autopilot modes.