Paparazzi UAS  v5.15_devel-105-g9de12e2
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
demo_ahrs_actuators.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
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 
25 #include <inttypes.h>
26 
27 /* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h
28  * in order to implement telemetry_mode_Main_*
29  */
30 #define PERIODIC_C_MAIN
31 #define ABI_C
32 #define MODULES_C
33 
37 #include "subsystems/abi.h"
38 
39 #include "generated/airframe.h"
40 #include "generated/settings.h"
41 #include "generated/modules.h"
42 
43 #include "std.h"
44 #include "mcu.h"
45 #include "mcu_periph/sys_time.h"
46 #include "led.h"
47 
48 #include "state.h"
49 #include "subsystems/imu.h"
50 #include "subsystems/ahrs.h"
51 
52 #include "subsystems/commands.h"
53 #include "subsystems/actuators.h"
54 #include "subsystems/settings.h"
55 
56 #include "pprz_version.h"
57 
58 #ifndef DEMO_MAX_ROLL
59 #define DEMO_MAX_ROLL RadOfDeg(65)
60 #endif
61 
62 #ifndef DEMO_MAX_PITCH
63 #define DEMO_MAX_PITCH RadOfDeg(65)
64 #endif
65 
66 static inline void main_init(void);
67 static inline void main_periodic_task(void);
68 static inline void main_event_task(void);
69 
70 static void send_alive(struct transport_tx *trans, struct link_device *dev);
71 static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
72 static void send_actuators(struct transport_tx *trans, struct link_device *dev);
73 static void send_commands(struct transport_tx *trans, struct link_device *dev);
74 
75 int main(void)
76 {
77  main_init();
78  while (1) {
81  }
83  }
84  return 0;
85 }
86 
87 static inline void main_init(void)
88 {
89  mcu_init();
90  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
91 
92  stateInit();
93  actuators_init();
94 
95  modules_init();
96 
97  ahrs_init();
98 
99  settings_init();
100 
101  mcu_int_enable();
102 
103  downlink_init();
104 
105  modules_init();
106 
107  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
111 
112  // send body_to_imu from here for now
113  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
114 }
115 
116 static inline void main_periodic_task(void)
117 {
118  /* Simply set current roll/pitch as commands.
119  * Scale DEMO_MAX_ROLL/PITCH to MAX_PPRZ (the max commands)
120  */
123 
124  /* generated macro from airframe file, seconds AP_MODE param not used */
125  SetActuatorsFromCommands(commands, 0);
126 
127  if (sys_time.nb_sec > 1) {
128  modules_periodic_task();
129  }
130  RunOnceEvery(10, { LED_PERIODIC();});
131  RunOnceEvery(PERIODIC_FREQUENCY, { datalink_time++; });
132  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
133 
134  modules_periodic_task();
135 }
136 
137 static inline void main_event_task(void)
138 {
139  mcu_event();
140  modules_event_task();
141 }
142 
143 static void send_alive(struct transport_tx *trans, struct link_device *dev)
144 {
145  pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
146 }
147 
148 void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
149 {
150  static uint32_t ap_version = PPRZ_VERSION_INT;
151  static char *ver_desc = PPRZ_VERSION_DESC;
152  pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
153 }
154 
155 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
156 {
157  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
158 }
159 
160 static void send_commands(struct transport_tx *trans, struct link_device *dev)
161 {
162  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands);
163 }
int main(void)
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:83
Dispatcher to register actual AHRS implementations.
float phi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
#define DEMO_MAX_ROLL
Periodic telemetry system header (includes downlink utility and generated code).
Main include for ABI (AirBorneInterface).
static void main_periodic_task(void)
struct Imu imu
global IMU state
Definition: imu.c:108
#define mcu_int_enable()
Definition: mcu_arch.h:36
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
void ahrs_init(void)
AHRS initialization.
Definition: ahrs.c:73
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
void settings_init(void)
Definition: settings.c:43
void stateInit(void)
Definition: state.c:43
float theta
in radians
Hardware independent API for actuators (servos, motor controllers).
static void main_event_task(void)
Architecture independent timing functions.
static void send_alive(struct transport_tx *trans, struct link_device *dev)
unsigned long uint32_t
Definition: types.h:18
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
Inertial Measurement Unit interface.
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
uint16_t datalink_time
Definition: sim_ap.c:41
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Arch independent mcu ( Micro Controller Unit ) utilities.
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
API to get/set the generic vehicle states.
Persistent settings interface.
static void main_init(void)
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:114
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
static void send_commands(struct transport_tx *trans, struct link_device *dev)
#define LED_PERIODIC()
Definition: led_hw.h:55
arch independent LED (Light Emitting Diodes) API
void mcu_event(void)
MCU event functions.
Definition: mcu.c:248
#define MAX_PPRZ
Definition: paparazzi.h:8
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).