Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/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 "modules/imu/imu.h"
50 #include "modules/ahrs/ahrs.h"
51 
52 #include "modules/core/commands.h"
54 #include "modules/core/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();
94 
95  modules_init();
96 
97  ahrs_init();
98 
99  settings_init();
100 
101 
102  downlink_init();
103 
104  modules_init();
105 
106  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
110 
111  // send body_to_imu from here for now
112  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
113 }
114 
115 static inline void main_periodic_task(void)
116 {
117  /* Simply set current roll/pitch as commands.
118  * Scale DEMO_MAX_ROLL/PITCH to MAX_PPRZ (the max commands)
119  */
122 
123  /* generated macro from airframe file, seconds AP_MODE param not used */
124  SetActuatorsFromCommands(commands, 0);
125 
126  if (sys_time.nb_sec > 1) {
127  modules_periodic_task();
128  }
129  RunOnceEvery(10, { LED_PERIODIC();});
130  RunOnceEvery(PERIODIC_FREQUENCY, { datalink_time++; });
131  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
132 
133  modules_periodic_task();
134 }
135 
136 static inline void main_event_task(void)
137 {
138  mcu_event();
139  modules_event_task();
140 }
141 
142 static void send_alive(struct transport_tx *trans, struct link_device *dev)
143 {
144  pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
145 }
146 
147 void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
148 {
149  static uint32_t ap_version = PPRZ_VERSION_INT;
150  static char *ver_desc = PPRZ_VERSION_DESC;
151  pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
152 }
153 
154 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
155 {
156  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
157 }
158 
159 static void send_commands(struct transport_tx *trans, struct link_device *dev)
160 {
161  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands);
162 }
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
stateInit
void stateInit(void)
Definition: state.c:43
settings.h
abi.h
mcu_event
void mcu_event(void)
MCU event functions.
Definition: mcu.c:266
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Imu::body_to_imu
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
commands
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
send_alive
static void send_alive(struct transport_tx *trans, struct link_device *dev)
Definition: demo_ahrs_actuators.c:142
ahrs_init
void ahrs_init(void)
AHRS initialization.
Definition: ahrs.c:73
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
telemetry.h
imu.h
std.h
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
send_commands
static void send_commands(struct transport_tx *trans, struct link_device *dev)
Definition: demo_ahrs_actuators.c:159
settings_init
void settings_init(void)
Definition: settings.c:46
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
send_autopilot_version
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
Definition: demo_ahrs_actuators.c:147
DEMO_MAX_ROLL
#define DEMO_MAX_ROLL
Definition: demo_ahrs_actuators.c:59
mcu.h
Arch independent mcu ( Micro Controller Unit ) utilities.
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
sys_time.h
Architecture independent timing functions.
LED_PERIODIC
#define LED_PERIODIC()
Definition: led_hw.h:55
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
main_event_task
static void main_event_task(void)
Definition: demo_ahrs_actuators.c:136
led.h
arch independent LED (Light Emitting Diodes) API
ahrs.h
imu
struct Imu imu
global IMU state
Definition: imu.c:108
main_init
static void main_init(void)
Definition: demo_ahrs_actuators.c:87
datalink_time
uint16_t datalink_time
Definition: sim_ap.c:40
main_periodic_task
static void main_periodic_task(void)
Definition: demo_ahrs_actuators.c:115
actuators.h
sys_time
Definition: sys_time.h:71
sys_time_register_timer
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
send_actuators
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
Definition: demo_ahrs_actuators.c:154
commands.h
Hardware independent code for commands handling.
state.h
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
actuators_init
void actuators_init(void)
Definition: actuators.c:122
mcu_init
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:97
main
int main(void)
Definition: demo_ahrs_actuators.c:75
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
sys_time_check_and_ack_timer
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:123