Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 
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 
42 
43 
44 #include "mcu.h"
45 #include "mcu_periph/sys_time.h"
46 #include "led.h"
47 
48 #include "subsystems/actuators.h"
49 
50 
51 static inline void main_init(void);
52 static inline void main_periodic(void);
53 static inline void main_event(void);
54 
55 int main(void)
56 {
57 
58  main_init();
59  while (1) {
61  main_periodic();
62  }
63  main_event();
64  };
65  return 0;
66 }
67 
68 
69 static inline void main_init(void)
70 {
71  mcu_init();
72 
73  downlink_init();
74 
75  actuators_init();
76  uint8_t i;
77  for (i = 0; i < ACTUATORS_NB; i++) {
78  //SetServo(i, 1500);
79  }
80 
82  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
83 }
84 
85 
86 static inline void main_periodic(void)
87 {
88 
89  // generated macro from airframe file
90  AllActuatorsCommit();
91 
92  LED_PERIODIC();
93  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
94  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators));
95 }
96 
97 static inline void main_event(void)
98 {
99  mcu_event();
100 
101  DatalinkEvent();
102 }
103 
104 
105 #define IdOfMsg(x) (x[1])
106 
107 void dl_parse_msg(void)
108 {
109  uint8_t msg_id = IdOfMsg(dl_buffer);
110  if (msg_id == DL_SET_ACTUATOR) {
111  uint8_t actuator_no = DL_SET_ACTUATOR_no(dl_buffer);
112  uint16_t actuator_value __attribute__((unused)) = DL_SET_ACTUATOR_value(dl_buffer);
113  LED_TOGGLE(2);
114 
115  /* bad hack:
116  * first arg to ActuatorSet needs to be the servo _name_ as given in the airframe file
117  * here we rely on the servos having number as names in the setup.xml airframe file
118  */
119  switch (actuator_no) {
120 #ifdef SERVO_0
121  case 0: ActuatorSet(0, actuator_value); break;
122 #endif
123 #ifdef SERVO_1
124  case 1: ActuatorSet(1, actuator_value); break;
125 #endif
126 #ifdef SERVO_2
127  case 2: ActuatorSet(2, actuator_value); break;
128 #endif
129 #ifdef SERVO_3
130  case 3: ActuatorSet(3, actuator_value); break;
131 #endif
132 #ifdef SERVO_4
133  case 4: ActuatorSet(4, actuator_value); break;
134 #endif
135 #ifdef SERVO_5
136  case 5: ActuatorSet(5, actuator_value); break;
137 #endif
138 #ifdef SERVO_6
139  case 6: ActuatorSet(6, actuator_value); break;
140 #endif
141 #ifdef SERVO_7
142  case 7: ActuatorSet(7, actuator_value); break;
143 #endif
144 #ifdef SERVO_8
145  case 8: ActuatorSet(8, actuator_value); break;
146 #endif
147  default: break;
148  }
149 
150  //if (actuator_no < ACTUATORS_NB) {
151  // actuators[actuator_no] = actuator_value;
152  //}
153  }
154 #ifdef DlSetting
155  else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
156  uint8_t i = DL_SETTING_index(dl_buffer);
157  float val = DL_SETTING_value(dl_buffer);
158  DlSetting(i, val);
159  LED_TOGGLE(2);
160 
161 #ifdef SERVO_0
162  ActuatorSet(0, actuators[SERVO_0_IDX]);
163 #endif
164 #ifdef SERVO_1
165  ActuatorSet(1, actuators[SERVO_1_IDX]);
166 #endif
167 #ifdef SERVO_2
168  ActuatorSet(2, actuators[SERVO_2_IDX]);
169 #endif
170 #ifdef SERVO_3
171  ActuatorSet(3, actuators[SERVO_3_IDX]);
172 #endif
173 #ifdef SERVO_4
174  ActuatorSet(4, actuators[SERVO_4_IDX]);
175 #endif
176 #ifdef SERVO_5
177  ActuatorSet(5, actuators[SERVO_5_IDX]);
178 #endif
179 #ifdef SERVO_6
180  ActuatorSet(6, actuators[SERVO_6_IDX]);
181 #endif
182 #ifdef SERVO_7
183  ActuatorSet(7, actuators[SERVO_7_IDX]);
184 #endif
185 #ifdef SERVO_8
186  ActuatorSet(8, actuators[SERVO_8_IDX]);
187 #endif
188 
189  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
190  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
191  uint8_t i = DL_GET_SETTING_index(dl_buffer);
192  float val = settings_get_value(i);
193  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
194  }
195 #endif
196 }
unsigned short uint16_t
Definition: types.h:16
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:76
#define mcu_int_enable()
Definition: mcu_arch.h:38
static void main_event(void)
Hardware independent API for actuators (servos, motor controllers).
Architecture independent timing functions.
uint16_t val[TCOUPLE_NB]
int main(void)
#define LED_TOGGLE(i)
Definition: led_hw.h:51
Arch independent mcu ( Micro Controller Unit ) utilities.
unsigned char uint8_t
Definition: types.h:14
static void main_periodic(void)
void dl_parse_msg(void)
Should be called when chars are available in dl_buffer.
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:52
#define IdOfMsg(x)
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
arch independent LED (Light Emitting Diodes) API
void mcu_event(void)
MCU event functions.
Definition: mcu.c:217
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43