Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
actuators.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
3  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
29 #include <sys/ioctl.h>
30 #include <fcntl.h>
31 
32 /* build ioctl unique identifiers for R/W operations */
33 #define PWM_MAGIC 'p'
34 typedef struct { unsigned int val[4]; } __attribute__ ((packed)) pwm_delos_quadruplet;
35 #define PWM_DELOS_SET_RATIOS _IOR(PWM_MAGIC, 9, pwm_delos_quadruplet*)
36 #define PWM_DELOS_SET_SPEEDS _IOR(PWM_MAGIC, 10, pwm_delos_quadruplet*)
37 #define PWM_DELOS_SET_CTRL _IOR(PWM_MAGIC, 11, unsigned int)
38 #define PWM_DELOS_REQUEST _IO(PWM_MAGIC, 12)
39 
40 #define PWM_NB_BITS (9)
41 
42 /* PWM can take value between 0 and 511 */
43 #ifndef PWM_TOTAL_RANGE
44 #define PWM_TOTAL_RANGE (1<<PWM_NB_BITS)
45 #endif
46 
47 #define PWM_REG_RATIO_PRECISION_MASK (PWM_NB_BITS<<16)
48 #define PWM_REG_SATURATION (PWM_REG_RATIO_PRECISION_MASK|PWM_TOTAL_RANGE)
49 
50 #include "subsystems/actuators.h"
52 #include "actuators.h"
53 #include "autopilot.h"
54 
56 static int actuators_fd;
57 
58 // Start/stop PWM
59 enum {
60  SiP6_PWM0_START = (1<<0),
61  SiP6_PWM1_START = (1<<1),
62  SiP6_PWM2_START = (1<<2),
63  SiP6_PWM3_START = (1<<3),
64 };
65 
67 {
68  actuators_fd = open("/dev/pwm", O_RDWR);
69 
70  pwm_delos_quadruplet m = {{ 1, 1, 1, 1 }};
71  int ret __attribute__((unused)) = ioctl(actuators_fd, PWM_DELOS_SET_SPEEDS, &m);
72 #if ACTUATORS_SWING_DEBUG
73  printf("Return Speeds: %d\n", ret);
74 #endif
75 
77 
79 
80  ret = ioctl(actuators_fd, PWM_DELOS_SET_CTRL, &control_reg);
81 #if ACTUATORS_SWING_DEBUG
82  printf("Return control: %d\n", ret);
83 #endif
84 }
85 
87 {
89 
90  m.val[0] = actuators_swing.rpm_ref[0] & 0xffff;
91  m.val[1] = actuators_swing.rpm_ref[1] & 0xffff;
92  m.val[2] = actuators_swing.rpm_ref[2] & 0xffff;
93  m.val[3] = actuators_swing.rpm_ref[3] & 0xffff;
94 
95 
100 
101  if( actuators_swing.rpm_ref[0] < 0 ) { m.val[0] = 0; }
102  if( actuators_swing.rpm_ref[1] < 0 ) { m.val[1] = 0; }
103  if( actuators_swing.rpm_ref[2] < 0 ) { m.val[2] = 0; }
104  if( actuators_swing.rpm_ref[3] < 0 ) { m.val[3] = 0; }
105 
106  /* The upper 16-bit word of the ratio register contains the number
107  * of bits used to code the ratio command */
112 
113  int ret __attribute__((unused)) = ioctl(actuators_fd, PWM_DELOS_SET_RATIOS, &m);
114 
115 #if ACTUATORS_SWING_DEBUG
116  RunOnceEvery(512, printf("Return ratios: %d (ratios: %d %d %d %d, pwm: %d %d %d %d\n",
117  ret,
118  m.val[0], m.val[1], m.val[2], m.val[3],
123  );
124 #endif
125 }
126 
SiP6_PWM0_START
@ SiP6_PWM0_START
Definition: actuators.c:60
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
motor_mixing.h
PWM_REG_RATIO_PRECISION_MASK
#define PWM_REG_RATIO_PRECISION_MASK
Definition: actuators.c:47
actuators_fd
static int actuators_fd
Definition: actuators.c:56
actuators_swing_init
void actuators_swing_init(void)
Definition: actuators.c:66
SiP6_PWM3_START
@ SiP6_PWM3_START
Definition: actuators.c:63
pwm_delos_quadruplet::val
unsigned int val[4]
Definition: actuators.c:34
PWM_DELOS_SET_SPEEDS
#define PWM_DELOS_SET_SPEEDS
Definition: actuators.c:36
SiP6_PWM1_START
@ SiP6_PWM1_START
Definition: actuators.c:61
actuators_swing
struct ActuatorsSwing actuators_swing
Definition: actuators.c:55
ActuatorsSwing
Definition: actuators.h:33
PWM_REG_SATURATION
#define PWM_REG_SATURATION
Definition: actuators.c:48
PWM_DELOS_SET_CTRL
#define PWM_DELOS_SET_CTRL
Definition: actuators.c:37
pwm_delos_quadruplet
Definition: actuators.c:34
autopilot.h
PWM_DELOS_SET_RATIOS
#define PWM_DELOS_SET_RATIOS
Definition: actuators.c:35
actuators_swing_commit
void actuators_swing_commit(void)
Definition: actuators.c:86
PWM_TOTAL_RANGE
#define PWM_TOTAL_RANGE
Definition: actuators.c:44
SiP6_PWM2_START
@ SiP6_PWM2_START
Definition: actuators.c:62
ActuatorsSwing::rpm_ref
uint16_t rpm_ref[4]
Reference RPM.
Definition: actuators.h:34
actuators.h
actuators.h