Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
autopilot_firmware.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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 */
20
27
29
30#include "generated/modules.h"
31
32#include <stdint.h>
36
37#if USE_GPS
38#include "modules/gps/gps.h"
39#else
40#if NO_GPS_NEEDED_FOR_NAV
41#define GpsIsLost() FALSE
42#else
43#define GpsIsLost() TRUE
44#endif
45#endif
46
47
48/* Geofence exceptions */
50
51#if USE_MOTOR_MIXING
53#endif
54
55static void send_status(struct transport_tx *trans, struct link_device *dev)
56{
57#if USE_GPS
58 uint8_t fix = gps.fix;
59#else
60 uint8_t fix = 0;
61#endif
62 uint8_t motors_on = autopilot.motors_on;
63 uint16_t time_sec = sys_time.nb_sec;
64 pprz_msg_send_ROVER_STATUS(trans, dev, AC_ID,
65 &radio_control.status, &radio_control.frame_rate,
66 &fix, &autopilot.mode, &motors_on,
67 &time_sec, &electrical.vsupply);
68}
69
70static void send_energy(struct transport_tx *trans, struct link_device *dev)
71{
72 uint8_t throttle = 100 * autopilot.throttle / MAX_PPRZ;
73 float power = electrical.vsupply * electrical.current;
74 float avg_power = 0;
75 if(electrical.avg_cnt != 0) {
76 avg_power = (float)electrical.avg_power / electrical.avg_cnt;
77 }
78
79 pprz_msg_send_ENERGY(trans, dev, AC_ID,
80 &throttle, &electrical.vsupply, &electrical.current, &power, &avg_power, &electrical.charge, &electrical.energy);
81}
82
83static void send_fp(struct transport_tx *trans, struct link_device *dev)
84{
85 int32_t carrot_east = POS_BFP_OF_REAL(nav.carrot.x);
86 int32_t carrot_north = POS_BFP_OF_REAL(nav.carrot.y);
87 int32_t carrot_up = 0;
88 int32_t carrot_heading = ANGLE_BFP_OF_REAL(nav.heading);
89 int32_t cmd = (int32_t)autopilot.throttle;
90 pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
94 &(stateGetSpeedEnu_i()->x),
95 &(stateGetSpeedEnu_i()->y),
96 &(stateGetSpeedEnu_i()->z),
98 &(stateGetNedToBodyEulers_i()->theta),
100 &carrot_east,
101 &carrot_north,
102 &carrot_up,
103 &carrot_heading,
104 &cmd,
105 &autopilot.flight_time);
106}
107
108
109#ifdef RADIO_CONTROL
110static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
111{
112#ifdef RADIO_KILL_SWITCH
113 int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH];
114#else
115 int16_t _kill_switch = 42;
116#endif
117 pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
118 &radio_control.values[RADIO_ROLL],
119 &radio_control.values[RADIO_PITCH],
120 &radio_control.values[RADIO_YAW],
122 &radio_control.values[RADIO_MODE],
123 &_kill_switch,
124 &radio_control.status);
125}
126#endif
127
129{
130 // register messages
131 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROVER_STATUS, send_status);
133 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
134#ifdef RADIO_CONTROL
135 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
136#endif
137}
138
143{
144}
145
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
static struct uart_periph * dev
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
void autopilot_firmware_init(void)
Init function.
static void send_energy(struct transport_tx *trans, struct link_device *dev)
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface).
#define ANGLE_BFP_OF_REAL(_af)
#define POS_BFP_OF_REAL(_af)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1307
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition state.h:814
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition state.h:1024
Motor Mixing.
Optional exceptions triggeringg HOME_MODE 1) GEOFENCE_DATALINK_LOST_TIME: go to HOME mode if datalink...
#define MAX_PPRZ
Definition paparazzi.h:8
struct RadioControl radio_control
Generic interface for radio control modules.
void autopilot_event(void)
autopilot event function
static void send_status(struct transport_tx *trans, struct link_device *dev)
static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
static void send_fp(struct transport_tx *trans, struct link_device *dev)
struct RotorcraftNavigation nav
Definition navigation.c:51
Rover specific autopilot interface and initialization.
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.