Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pwm_sysfs.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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  */
20 
27 #include "mcu_periph/pwm_sysfs.h"
28 
29 #include <errno.h>
30 #include <fcntl.h>
31 #include <inttypes.h>
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <unistd.h>
35 #include <stdarg.h>
36 
37 // default period in nano seconds (100Hz)
38 #define PWM_SYSFS_DEFAULT_PERIOD 10000000
39 
40 // print debug
41 #if PWM_SYSFS_DEBUG
42 #define PS_DEBUG_PRINT printf
43 #else
44 #define PS_DEBUG_PRINT(...) {}
45 #endif
46 
47 // Write in file
48 static int write_file(const char *path, const char *fmt, ...);
49 // Read file content
50 static int read_file(const char *path, const char *fmt, ...) __attribute__((unused));
51 
52 int pwm_sysfs_init(struct PWM_Sysfs *pwm, char *base_path,
53  char *_export, char *_enable,
54  char *_duty, char *_period,
55  uint8_t channel)
56 {
57  // path to export file (create new PWM)
58  char export_path[PWM_SYSFS_PATH_LEN];
59 
60  if (base_path == NULL || _export == NULL || _enable == NULL ||
61  _period == NULL || _duty == NULL) {
62  // invalid paths
63  PS_DEBUG_PRINT("invalid paths\n");
64  return -1;
65  }
66  // store paths
67  snprintf(export_path, PWM_SYSFS_PATH_LEN, "%s/%s", base_path, _export);
68  snprintf(pwm->enable_path, PWM_SYSFS_PATH_LEN, "%s/pwm_%u/%s", base_path, channel, _enable);
69  snprintf(pwm->duty_path, PWM_SYSFS_PATH_LEN, "%s/pwm_%u/%s", base_path, channel, _duty);
70  snprintf(pwm->period_path, PWM_SYSFS_PATH_LEN, "%s/pwm_%u/%s", base_path, channel, _period);
71 
72  PS_DEBUG_PRINT("%s\n%s\n%s\n%s\n",
73  export_path, pwm->enable_path, pwm->duty_path, pwm->period_path);
74 
75  // export new PWM entry
76  write_file(export_path, "%u", channel);
77  // open duty cycle file
78  pwm->duty_cycle_fd = open(pwm->duty_path, O_RDWR | O_CLOEXEC);
79  if (pwm->duty_cycle_fd < 0) {
80  // fail to open duty file
81  PS_DEBUG_PRINT("failed to open FD cycle: %d\n", pwm->duty_cycle_fd);
82  return -2;
83  }
84  // set default period
86 
87  // init with sucess
88  return 0;
89 }
90 
91 void pwm_sysfs_set_period(struct PWM_Sysfs *pwm, uint32_t period)
92 {
93  if (write_file(pwm->period_path, "%u", period) < 0) {
94  PS_DEBUG_PRINT("failed to set period\n");
95  return;
96  }
97  PS_DEBUG_PRINT("setting period %d\n", period);
98  pwm->period_nsec = period;
99 }
100 
101 void pwm_sysfs_set_duty(struct PWM_Sysfs *pwm, uint32_t duty)
102 {
103  if (dprintf(pwm->duty_cycle_fd, "%u", duty) < 0) {
104  PS_DEBUG_PRINT("failed to set duty %u; %d; %s\n", duty, pwm->duty_cycle_fd, pwm->duty_path);
105  return;
106  }
107  pwm->duty_cycle_nsec = duty;
108  // enable pwm if not done
109  if (!pwm->enabled) {
110  pwm_sysfs_enable(pwm, true);
111  }
112 }
113 
114 void pwm_sysfs_enable(struct PWM_Sysfs *pwm, bool enable)
115 {
116  int en = enable ? 1 : 0;
117  int ret = write_file(pwm->enable_path, "%u", en);
118  if (ret < 0) {
119  PS_DEBUG_PRINT("failed to enable: %d; %s\n", ret, pwm->enable_path);
120  return;
121  }
122  PS_DEBUG_PRINT("enable channel: %d; %s\n", en, pwm->enable_path);
123  pwm->enabled = enable;
124 }
125 
126 
127 static int write_file(const char *path, const char *fmt, ...)
128 {
129  errno = 0;
130 
131  int fd = open(path, O_WRONLY | O_CLOEXEC);
132  if (fd == -1) {
133  return -errno;
134  }
135 
136  va_list args;
137  va_start(args, fmt);
138 
139  int ret = vdprintf(fd, fmt, args);
140  int errno_bkp = errno;
141  close(fd);
142 
143  va_end(args);
144 
145  if (ret < 1) {
146  return -errno_bkp;
147  }
148 
149  return ret;
150 }
151 
152 static int read_file(const char *path, const char *fmt, ...)
153 {
154  errno = 0;
155 
156  FILE *file = fopen(path, "re");
157  if (!file) {
158  return -errno;
159  }
160 
161  va_list args;
162  va_start(args, fmt);
163 
164  int ret = vfscanf(file, fmt, args);
165  int errno_bkp = errno;
166  fclose(file);
167 
168  va_end(args);
169 
170  if (ret < 1) {
171  return -errno_bkp;
172  }
173 
174  return ret;
175 }
176 
177 
void pwm_sysfs_set_period(struct PWM_Sysfs *pwm, uint32_t period)
Definition: pwm_sysfs.c:91
#define PS_DEBUG_PRINT(...)
Definition: pwm_sysfs.c:44
void pwm_sysfs_set_duty(struct PWM_Sysfs *pwm, uint32_t duty)
Definition: pwm_sysfs.c:101
void pwm_sysfs_enable(struct PWM_Sysfs *pwm, bool enable)
Definition: pwm_sysfs.c:114
static int read_file(const char *path, const char *fmt,...)
Definition: pwm_sysfs.c:152
#define PWM_SYSFS_DEFAULT_PERIOD
Definition: pwm_sysfs.c:38
static int write_file(const char *path, const char *fmt,...)
Definition: pwm_sysfs.c:127
int pwm_sysfs_init(struct PWM_Sysfs *pwm, char *base_path, char *_export, char *_enable, char *_duty, char *_period, uint8_t channel)
Definition: pwm_sysfs.c:52
PWM servos handling using Linux sysfs.
char enable_path[PWM_SYSFS_PATH_LEN]
path to enable file
Definition: pwm_sysfs.h:41
int duty_cycle_fd
file descriptor to write/update duty cycle
Definition: pwm_sysfs.h:40
uint32_t duty_cycle_nsec
current duty cycle (in nsec)
Definition: pwm_sysfs.h:37
uint32_t period_nsec
current period (in nsec)
Definition: pwm_sysfs.h:38
char duty_path[PWM_SYSFS_PATH_LEN]
path to duty file
Definition: pwm_sysfs.h:42
char period_path[PWM_SYSFS_PATH_LEN]
path to period file
Definition: pwm_sysfs.h:43
#define PWM_SYSFS_PATH_LEN
Definition: pwm_sysfs.h:32
bool enabled
true if pwm is enabled
Definition: pwm_sysfs.h:39
PWM structure.
Definition: pwm_sysfs.h:36
int fd
Definition: serial.c:26
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98