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
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 
30 
31 #include "state.h"
34 #include "generated/settings.h"
35 
36 #ifdef POWER_SWITCH_GPIO
37 #include "mcu_periph/gpio.h"
38 #endif
39 
43 
44 bool_t launch;
45 
48 
50 
53 float energy;
54 
55 bool_t gps_lost;
56 
57 bool_t power_switch;
58 
59 static void send_alive(void) {
60  DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
61 }
62 
63 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
64 #include "rc_settings.h"
65 static void send_rc_settings(void) {
66  if (!RcSettingsOff())
67  DOWNLINK_SEND_SETTINGS(DefaultChannel, DefaultDevice, &slider_1_val, &slider_2_val);
68 }
69 #else
71 #endif
72 
73 void autopilot_send_mode(void) {
74  DOWNLINK_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice,
75  &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
76 }
77 
78 static void send_attitude(void) {
79  struct FloatEulers* att = stateGetNedToBodyEulers_f();
80  DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice,
81  &(att->phi), &(att->psi), &(att->theta));
82 };
83 
84 static void send_estimator(void) {
85  DOWNLINK_SEND_ESTIMATOR(DefaultChannel, DefaultDevice,
86  &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
87 }
88 
89 static void send_bat(void) {
90  int16_t amps = (int16_t) (current/10);
91  int16_t e = energy;
92  DOWNLINK_SEND_BAT(DefaultChannel, DefaultDevice,
95  &block_time, &stage_time, &e);
96 }
97 
98 static void send_energy(void) {
99  const int16_t e = energy;
100  const float vsup = ((float)vsupply) / 10.0f;
101  const float curs = ((float)current) / 1000.0f;
102  const float power = vsup * curs;
103  DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power);
104 }
105 
106 static void send_dl_value(void) {
107  PeriodicSendDlValue(DefaultChannel, DefaultDevice);
108 }
109 
110 // FIXME not the best place
112 #include CTRL_TYPE_H
113 static void send_desired(void) {
114 #ifndef USE_AIRSPEED
115  float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
116 #endif
117  DOWNLINK_SEND_DESIRED(DefaultChannel, DefaultDevice,
120  &v_ctl_auto_airspeed_setpoint);
121 }
122 
123 static void send_airspeed(void) {
124 #ifdef MEASURE_AIRSPEED
125  float* airspeed = stateGetAirspeed_f();
126  DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, airspeed, airspeed, airspeed, airspeed);
127 #elif USE_AIRSPEED
128  DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice,
131 #endif
132 }
133 
134 static void send_downlink(void) {
135  static uint16_t last;
136  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0;
137  last = downlink_nb_bytes;
138  DOWNLINK_SEND_DOWNLINK(DefaultChannel, DefaultDevice,
140 }
141 
142 void autopilot_init(void) {
145  launch = FALSE;
147 
149 
150  gps_lost = FALSE;
151 
153 #ifdef POWER_SWITCH_GPIO
156 #endif
157 
158  /* register some periodic message */
159  register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
160  register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", autopilot_send_mode);
161  register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink);
162  register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
163  register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator);
164  register_periodic_telemetry(DefaultPeriodic, "AIRSPEED", send_airspeed);
165  register_periodic_telemetry(DefaultPeriodic, "BAT", send_bat);
166  register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy);
167  register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
168  register_periodic_telemetry(DefaultPeriodic, "DESIRED", send_desired);
169 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
170  register_periodic_telemetry(DefaultPeriodic, "RC_SETTINGS", send_rc_settings);
171 #endif
172 }
173 
uint16_t block_time
unsigned short uint16_t
Definition: types.h:16
uint16_t vsupply
Supply voltage in deciVolt.
Definition: autopilot.c:51
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:91
#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:1027
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:142
#define LATERAL_MODE_MANUAL
Definition: autopilot.h:74
Variable setting though the radio control.
Periodic telemetry system header (includes downlink utility and generated code).
#define RcSettingsOff()
Definition: rc_settings.h:70
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:47
float h_ctl_course_setpoint
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:128
uint8_t pprz_mode
Definition: autopilot.c:40
float psi
in radians
uint8_t lateral_mode
Definition: autopilot.c:49
Some architecture independent helper functions for GPIOs.
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:101
static void send_energy(void)
Definition: autopilot.c:98
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:52
static void send_estimator(void)
Definition: autopilot.c:84
float theta
in radians
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
static void send_desired(void)
Definition: autopilot.c:113
int32_t current
Supply current in milliAmpere.
Definition: autopilot.c:52
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
#define FALSE
Definition: imu_chimu.h:141
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
Fixed wing horizontal control.
static void send_alive(void)
Definition: autopilot.c:59
bool_t gps_lost
Definition: autopilot.c:55
float phi
in radians
float h_ctl_pitch_loop_setpoint
signed short int16_t
Definition: types.h:17
bool_t kill_throttle
Definition: autopilot.c:41
float slider_1_val
Definition: rc_settings.c:35
static void send_dl_value(void)
Definition: autopilot.c:106
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77
uint16_t stage_time
In s.
signed long int32_t
Definition: types.h:19
void gpio_clear(uint32_t port, uint16_t pin)
Clear a gpio output to low level.
uint8_t rc_settings_mode
Definition: autopilot.c:70
void autopilot_send_mode(void)
Send mode over telemetry.
Definition: autopilot.c:73
float energy
Energy consumption (mAh) This is the ap copy of the measurement from fbw.
Definition: autopilot.c:53
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
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
static void gpio_setup_output(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.h:76
bool_t launch
Definition: autopilot.c:44
float h_ctl_roll_setpoint
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:840
static void send_bat(void)
Definition: autopilot.c:89
static void send_airspeed(void)
Definition: autopilot.c:123
uint8_t mcu1_status
Definition: autopilot.c:42
static void send_downlink(void)
Definition: autopilot.c:134
static void send_attitude(void)
Definition: autopilot.c:78
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:135
bool_t power_switch
Power switch control.
Definition: autopilot.c:57
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:126
Fixedwing autopilot modes.