Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 PERIODIC_C_MAIN
27 
28 #include "generated/airframe.h"
29 #include "generated/settings.h"
30 #include "generated/periodic_telemetry.h"
31 
34 
35 
36 #include "mcu.h"
37 #include "mcu_periph/sys_time.h"
38 #include "led.h"
39 
40 #include "subsystems/actuators.h"
41 
42 
43 static inline void main_init( void );
44 static inline void main_periodic( void );
45 static inline void main_event(void);
46 
47 int main(void) {
48 
49  main_init();
50  while (1) {
52  main_periodic();
53  main_event();
54  };
55  return 0;
56 }
57 
58 
59 static inline void main_init( void ) {
60  mcu_init();
61 
63  uint8_t i;
64  for(i = 0; i < ACTUATORS_NB; i++) {
65  //SetServo(i, 1500);
66  }
67 
70 }
71 
72 
73 static inline void main_periodic( void ) {
74 
75  // generated macro from airframe file
76  AllActuatorsCommit();
77 
78  LED_PERIODIC();
79  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
80  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators ));
81 }
82 
83 static inline void main_event(void) {
84  DatalinkEvent();
85 }
86 
87 
88 #define IdOfMsg(x) (x[1])
89 
90 void dl_parse_msg( void ) {
91  uint8_t msg_id = IdOfMsg(dl_buffer);
92  if (msg_id == DL_SET_ACTUATOR) {
93  uint8_t actuator_no = DL_SET_ACTUATOR_no(dl_buffer);
94  uint16_t actuator_value = DL_SET_ACTUATOR_value(dl_buffer);
95  LED_TOGGLE(2);
96 
97  /* bad hack:
98  * first arg to ActuatorSet needs to be the servo _name_ as given in the airframe file
99  * here we rely on the servos having number as names in the setup.xml airframe file
100  */
101  switch (actuator_no) {
102 #ifdef SERVO_0
103  case 0: ActuatorSet(0, actuator_value); break;
104 #endif
105 #ifdef SERVO_1
106  case 1: ActuatorSet(1, actuator_value); break;
107 #endif
108 #ifdef SERVO_2
109  case 2: ActuatorSet(2, actuator_value); break;
110 #endif
111 #ifdef SERVO_3
112  case 3: ActuatorSet(3, actuator_value); break;
113 #endif
114 #ifdef SERVO_4
115  case 4: ActuatorSet(4, actuator_value); break;
116 #endif
117 #ifdef SERVO_5
118  case 5: ActuatorSet(5, actuator_value); break;
119 #endif
120 #ifdef SERVO_6
121  case 6: ActuatorSet(6, actuator_value); break;
122 #endif
123 #ifdef SERVO_7
124  case 7: ActuatorSet(7, actuator_value); break;
125 #endif
126 #ifdef SERVO_8
127  case 8: ActuatorSet(8, actuator_value); break;
128 #endif
129  default: break;
130  }
131 
132  //if (actuator_no < ACTUATORS_NB) {
133  // actuators[actuator_no] = actuator_value;
134  //}
135  }
136 #ifdef DlSetting
137  else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
138  uint8_t i = DL_SETTING_index(dl_buffer);
139  float val = DL_SETTING_value(dl_buffer);
140  DlSetting(i, val);
141  LED_TOGGLE(2);
142 
143 #ifdef SERVO_0
144  ActuatorSet(0, actuators[SERVO_0_IDX];
145 #endif
146 #ifdef SERVO_1
147  ActuatorSet(1, actuators[SERVO_1_IDX]);
148 #endif
149 #ifdef SERVO_2
150  ActuatorSet(2, actuators[SERVO_2_IDX]);
151 #endif
152 #ifdef SERVO_3
153  ActuatorSet(3, actuators[SERVO_3_IDX]);
154 #endif
155 #ifdef SERVO_4
156  ActuatorSet(4, actuators[SERVO_4_IDX]);
157 #endif
158 #ifdef SERVO_5
159  ActuatorSet(5, actuators[SERVO_5_IDX]);
160 #endif
161 #ifdef SERVO_6
162  ActuatorSet(6, actuators[SERVO_6_IDX]);
163 #endif
164 #ifdef SERVO_7
165  ActuatorSet(7, actuators[SERVO_7_IDX]);
166 #endif
167 #ifdef SERVO_8
168  ActuatorSet(8, actuators[SERVO_8_IDX]);
169 #endif
170 
171  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
172  } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
173  uint8_t i = DL_GET_SETTING_index(dl_buffer);
174  float val = settings_get_value(i);
175  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
176  }
177 #endif
178 }
unsigned short uint16_t
Definition: types.h:16
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
static void main_event(void)
#define mcu_int_enable()
Definition: mcu_arch.h:37
Architecture independent timing functions.
uint16_t val[TCOUPLE_NB]
Hardware independent API for actuators (servos, motor controllers).
int main(void)
int sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:37
#define LED_TOGGLE(i)
Definition: led_hw.h:30
static bool_t sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:111
#define LED_PERIODIC()
Definition: led_hw.h:8
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)
static void main_init(void)
#define IdOfMsg(x)
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:59
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:51
arch independent LED (Light Emitting Diodes) API
void actuators_init(void)
Definition: actuators_at.c:32