Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
setup_actuators.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2013 The Paparazzi Team
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  */
25 #define DATALINK_C
26 #define MODULES_C
27 
28 /* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h
29  * in order to implement telemetry_mode_Main_*
30  */
31 #define PERIODIC_C_MAIN
32 
33 #pragma GCC diagnostic push
34 #pragma GCC diagnostic ignored "-Wunused-parameter"
35 #include "generated/periodic_telemetry.h"
36 #pragma GCC diagnostic pop
37 
38 #include "generated/airframe.h"
39 #include "generated/settings.h"
40 #include "generated/modules.h"
41 
45 
46 #include "mcu.h"
47 #include "mcu_periph/sys_time.h"
48 #include "led.h"
49 
50 #include "subsystems/actuators.h"
51 
52 
53 static inline void main_init(void);
54 static inline void main_periodic(void);
55 static inline void main_event(void);
56 
57 int main(void)
58 {
59 
60  main_init();
61  while (1) {
63  main_periodic();
64  }
65  main_event();
66  };
67  return 0;
68 }
69 
70 
71 static inline void main_init(void)
72 {
73  mcu_init();
74 
75  downlink_init();
76  pprz_dl_init();
77 
78  actuators_init();
79  uint8_t i;
80  for (i = 0; i < ACTUATORS_NB; i++) {
81  //SetServo(i, 1500);
82  }
83 
84  modules_init();
85 
87  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
88 }
89 
90 
91 static inline void main_periodic(void)
92 {
93 
94  // generated macro from airframe file
95  AllActuatorsCommit();
96 
97  LED_PERIODIC();
98  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
99  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators));
100 
101  modules_periodic_task();
102 }
103 
104 static inline void main_event(void)
105 {
106  mcu_event();
107  pprz_dl_event();
108  modules_event_task();
109 }
110 
111 
112 #define IdOfMsg(x) (x[1])
113 
114 void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
115 {
116  uint8_t msg_id = IdOfMsg(buf);
117  if (msg_id == DL_SET_ACTUATOR) {
118  uint8_t actuator_no = DL_SET_ACTUATOR_no(buf);
119  uint16_t actuator_value __attribute__((unused)) = DL_SET_ACTUATOR_value(buf);
120  LED_TOGGLE(2);
121 
122  /* bad hack:
123  * first arg to ActuatorSet needs to be the servo _name_ as given in the airframe file
124  * here we rely on the servos having number as names in the setup.xml airframe file
125  */
126  switch (actuator_no) {
127 #ifdef SERVO_0
128  case 0: ActuatorSet(0, actuator_value); break;
129 #endif
130 #ifdef SERVO_1
131  case 1: ActuatorSet(1, actuator_value); break;
132 #endif
133 #ifdef SERVO_2
134  case 2: ActuatorSet(2, actuator_value); break;
135 #endif
136 #ifdef SERVO_3
137  case 3: ActuatorSet(3, actuator_value); break;
138 #endif
139 #ifdef SERVO_4
140  case 4: ActuatorSet(4, actuator_value); break;
141 #endif
142 #ifdef SERVO_5
143  case 5: ActuatorSet(5, actuator_value); break;
144 #endif
145 #ifdef SERVO_6
146  case 6: ActuatorSet(6, actuator_value); break;
147 #endif
148 #ifdef SERVO_7
149  case 7: ActuatorSet(7, actuator_value); break;
150 #endif
151 #ifdef SERVO_8
152  case 8: ActuatorSet(8, actuator_value); break;
153 #endif
154  default: break;
155  }
156 
157  //if (actuator_no < ACTUATORS_NB) {
158  // actuators[actuator_no] = actuator_value;
159  //}
160  }
161 #ifdef DlSetting
162  else if (msg_id == DL_SETTING && DL_SETTING_ac_id(buf) == AC_ID) {
163  uint8_t i = DL_SETTING_index(buf);
164  float val = DL_SETTING_value(buf);
165  DlSetting(i, val);
166  LED_TOGGLE(2);
167 
168 #ifdef SERVO_0
169  ActuatorSet(0, actuators[SERVO_0_IDX]);
170 #endif
171 #ifdef SERVO_1
172  ActuatorSet(1, actuators[SERVO_1_IDX]);
173 #endif
174 #ifdef SERVO_2
175  ActuatorSet(2, actuators[SERVO_2_IDX]);
176 #endif
177 #ifdef SERVO_3
178  ActuatorSet(3, actuators[SERVO_3_IDX]);
179 #endif
180 #ifdef SERVO_4
181  ActuatorSet(4, actuators[SERVO_4_IDX]);
182 #endif
183 #ifdef SERVO_5
184  ActuatorSet(5, actuators[SERVO_5_IDX]);
185 #endif
186 #ifdef SERVO_6
187  ActuatorSet(6, actuators[SERVO_6_IDX]);
188 #endif
189 #ifdef SERVO_7
190  ActuatorSet(7, actuators[SERVO_7_IDX]);
191 #endif
192 #ifdef SERVO_8
193  ActuatorSet(8, actuators[SERVO_8_IDX]);
194 #endif
195 
196  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
197  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(buf) == AC_ID) {
198  uint8_t i = DL_GET_SETTING_index(buf);
199  float val = settings_get_value(i);
200  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
201  }
202 #endif
203 }
unsigned short uint16_t
Definition: types.h:16
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:76
void pprz_dl_init(void)
Init function.
Definition: pprz_dl.c:31
#define mcu_int_enable()
Definition: mcu_arch.h:36
Datalink using PPRZ protocol.
static void main_event(void)
Hardware independent API for actuators (servos, motor controllers).
Architecture independent timing functions.
uint16_t val[TCOUPLE_NB]
void pprz_dl_event(void)
Datalink Event.
Definition: pprz_dl.c:36
int main(void)
#define LED_TOGGLE(i)
Definition: led_hw.h:52
void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
Should be called when chars are available in dl_buffer.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Arch independent mcu ( Micro Controller Unit ) utilities.
unsigned char uint8_t
Definition: types.h:14
static void main_periodic(void)
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:114
static void main_init(void)
#define LED_PERIODIC()
Definition: led_hw.h:54
#define IdOfMsg(x)
arch independent LED (Light Emitting Diodes) API
void mcu_event(void)
MCU event functions.
Definition: mcu.c:231
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43