Paparazzi UAS  v6.1.0_stable
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 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 
42 #include "modules/datalink/datalink.h"
43 #include "modules/datalink/downlink.h"
44 #include "modules/datalink/pprz_dl.h"
45 
46 #include "mcu.h"
47 #include "mcu_periph/sys_time.h"
48 #include "led.h"
49 
50 #include "modules/actuators/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) {
62  if (sys_time_check_and_ack_timer(0)) {
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 
86  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
87 }
88 
89 
90 static inline void main_periodic(void)
91 {
92 
93  // generated macro from airframe file
94  AllActuatorsCommit();
95 
96  LED_PERIODIC();
97  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
98  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators));
99 
100  modules_periodic_task();
101 }
102 
103 static inline void main_event(void)
104 {
105  mcu_event();
106  pprz_dl_event();
107  modules_event_task();
108 }
109 
110 
111 #define IdOfMsg(x) (x[1])
112 
113 void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
114 {
115  uint8_t msg_id = IdOfMsg(buf);
116  if (msg_id == DL_SET_ACTUATOR) {
117  uint8_t actuator_no = DL_SET_ACTUATOR_no(buf);
118  uint16_t actuator_value __attribute__((unused)) = DL_SET_ACTUATOR_value(buf);
119  LED_TOGGLE(2);
120 
121  /* bad hack:
122  * first arg to ActuatorSet needs to be the servo _name_ as given in the airframe file
123  * here we rely on the servos having number as names in the setup.xml airframe file
124  */
125  switch (actuator_no) {
126 #ifdef SERVO_0
127  case 0: ActuatorSet(0, actuator_value); break;
128 #endif
129 #ifdef SERVO_1
130  case 1: ActuatorSet(1, actuator_value); break;
131 #endif
132 #ifdef SERVO_2
133  case 2: ActuatorSet(2, actuator_value); break;
134 #endif
135 #ifdef SERVO_3
136  case 3: ActuatorSet(3, actuator_value); break;
137 #endif
138 #ifdef SERVO_4
139  case 4: ActuatorSet(4, actuator_value); break;
140 #endif
141 #ifdef SERVO_5
142  case 5: ActuatorSet(5, actuator_value); break;
143 #endif
144 #ifdef SERVO_6
145  case 6: ActuatorSet(6, actuator_value); break;
146 #endif
147 #ifdef SERVO_7
148  case 7: ActuatorSet(7, actuator_value); break;
149 #endif
150 #ifdef SERVO_8
151  case 8: ActuatorSet(8, actuator_value); break;
152 #endif
153  default: break;
154  }
155 
156  //if (actuator_no < ACTUATORS_NB) {
157  // actuators[actuator_no] = actuator_value;
158  //}
159  }
160 #ifdef DlSetting
161  else if (msg_id == DL_SETTING && DL_SETTING_ac_id(buf) == AC_ID) {
162  uint8_t i = DL_SETTING_index(buf);
163  float val = DL_SETTING_value(buf);
164  DlSetting(i, val);
165  LED_TOGGLE(2);
166 
167 #ifdef SERVO_0
168  ActuatorSet(0, actuators[SERVO_0_IDX]);
169 #endif
170 #ifdef SERVO_1
171  ActuatorSet(1, actuators[SERVO_1_IDX]);
172 #endif
173 #ifdef SERVO_2
174  ActuatorSet(2, actuators[SERVO_2_IDX]);
175 #endif
176 #ifdef SERVO_3
177  ActuatorSet(3, actuators[SERVO_3_IDX]);
178 #endif
179 #ifdef SERVO_4
180  ActuatorSet(4, actuators[SERVO_4_IDX]);
181 #endif
182 #ifdef SERVO_5
183  ActuatorSet(5, actuators[SERVO_5_IDX]);
184 #endif
185 #ifdef SERVO_6
186  ActuatorSet(6, actuators[SERVO_6_IDX]);
187 #endif
188 #ifdef SERVO_7
189  ActuatorSet(7, actuators[SERVO_7_IDX]);
190 #endif
191 #ifdef SERVO_8
192  ActuatorSet(8, actuators[SERVO_8_IDX]);
193 #endif
194 
195  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
196  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(buf) == AC_ID) {
197  uint8_t i = DL_GET_SETTING_index(buf);
198  float val = settings_get_value(i);
199  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
200  }
201 #endif
202 }
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
pprz_dl_init
void pprz_dl_init(void)
Init function.
Definition: pprz_dl.c:35
IdOfMsg
#define IdOfMsg(x)
Definition: setup_actuators.c:111
mcu_event
void mcu_event(void)
MCU event functions.
Definition: mcu.c:258
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
main
int main(void)
Definition: setup_actuators.c:57
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
main_event
static void main_event(void)
Definition: setup_actuators.c:103
pprz_dl_event
void pprz_dl_event(void)
Datalink Event.
Definition: pprz_dl.c:40
mcu.h
Arch independent mcu ( Micro Controller Unit ) utilities.
main_periodic
static void main_periodic(void)
Definition: setup_actuators.c:90
sys_time.h
Architecture independent timing functions.
LED_PERIODIC
#define LED_PERIODIC()
Definition: led_hw.h:55
main_init
static void main_init(void)
Definition: setup_actuators.c:71
dl_parse_msg
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.
Definition: setup_actuators.c:113
led.h
arch independent LED (Light Emitting Diodes) API
pprz_dl.h
Datalink using PPRZ protocol.
actuators.h
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
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
mcu_init
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:87
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:119