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