add workaround to allow changing pwm frequency

Author: Demo User

examples/src/rc_test_motors.c

@@ -31,6 +31,7 @@ void print_usage()
 	printf("\n");
 	printf("-d {duty}   define a duty cycle from -1.0 to 1.0\n");
 	printf("-b          enable motor brake function\n");
+	printf("-F {freq}   set a custom pwm frequency in HZ, otherwise default 25000 is used\n");
 	printf("-f          enable free spin function\n");
 	printf("-s {duty}   sweep motors back and forward at duty cycle\n");
 	printf("-m {motor}  specify a single motor from 1-4, otherwise all will be driven\n");
@@ -51,11 +52,12 @@ int main(int argc, char *argv[])
 	double duty = 0.0;
 	int ch = 0; // assume all motor unless set otherwise
 	int c, in;
+	int freq_hz = RC_MOTOR_DEFAULT_PWM_FREQ;
 	m_mode_t m_mode = DISABLED;
 
 	// parse arguments
 	opterr = 0;
-	while ((c = getopt(argc, argv, "m:d:fbs:h")) != -1){
+	while ((c = getopt(argc, argv, "m:d:F:fbs:h")) != -1){
 		switch (c){
 		case 'm': // motor channel option
 			in = atoi(optarg);
@@ -78,6 +80,13 @@ int main(int argc, char *argv[])
 				return -1;
 			}
 			break;
+		case 'F': // pwm frequency option
+			freq_hz = atoi(optarg);
+			if(freq_hz<1){
+				fprintf(stderr,"PWM frequency must be >=1\n");
+				return -1;
+			}
+			break;
 		case 'f':
 			if(m_mode!=DISABLED) print_usage();
 			m_mode = FREE;
@@ -119,7 +128,7 @@ int main(int argc, char *argv[])
 	running =1;
 
 	// initialize hardware first
-	if(rc_motor_init()) return -1;
+	if(rc_motor_init_freq(freq_hz)) return -1;
 
 	// decide what to do
 	switch(m_mode){

library/Makefile

@@ -46,7 +46,7 @@ prefix		?= /usr
 $(TARGET): $(OBJECTS)
 	@mkdir -p $(LIBDIR)
 	@$(LINKER) -o $(TARGET) $(OBJECTS) $(LDFLAGS)
-	@ln -s $(FULLNAME) $(LIBDIR)/$(SONAME)
+	@ln -sf $(FULLNAME) $(LIBDIR)/$(SONAME)
 	@echo "Done making $(TARGET)"
 
 # rule for math libs
@@ -84,7 +84,7 @@ install:
 	@cp -r include/* $(DESTDIR)$(prefix)/include
 	@$(INSTALLDIR) $(DESTDIR)$(prefix)/lib
 	@$(INSTALL) $(TARGET) $(DESTDIR)$(prefix)/lib
-	@ln -s $(FULLNAME) $(DESTDIR)$(prefix)/lib/$(SONAME)
+	@ln -sf $(FULLNAME) $(DESTDIR)$(prefix)/lib/$(SONAME)
 	@echo "Library Install Complete"
 
 # cleanup local binaries

library/include/rc/motor.h

@@ -25,20 +25,44 @@
 extern "C" {
 #endif
 
+#define RC_MOTOR_DEFAULT_PWM_FREQ	25000	// 25kHz
+
 
 /**
  * @brief      Initializes all 4 motors and leaves them in a free-spin (0
  *             throttle) state.
  *
  *             Note, if the user is optionally using the rc_motor_standby
  *             functionality they should be aware that rc_motor_init starts
- *             standby mode in a disabled state so it is not necessary for the majority of users who are not interested in standby mode to
+ *             standby mode in a disabled state so it is not necessary for the
+ *             majority of users who are not interested in standby mode.
+ *
+ *             This starts the motor drivers at RC_MOTOR_DEFAULT_PWM_FREQ. To
+ *             use another frequency initialize with rc_motor_init_freq instead.
  *
  * @return     0 on success, -1 on failure which is usually due to lack of user
  *             permissions to access the gpio and pwm systems.
  */
 int rc_motor_init();
 
+/**
+ * @brief      Just like rc_motor_init but allows the user to set the pwm
+ *             frequency
+ *
+ *             RC_MOTOR_DEFAULT_PWM_FREQ is a good frequency to start at.
+ *
+ *             Note, if the user is optionally using the rc_motor_standby
+ *             functionality they should be aware that rc_motor_init starts
+ *             standby mode in a disabled state so it is not necessary for the
+ *             majority of users who are not interested in standby mode.
+ *
+ * @param[in]  pwm_frequency_hz  The pwm frequency in hz
+ *
+ * @return     0 on success, -1 on failure which is usually due to lack of user
+ *             permissions to access the gpio and pwm systems.
+ */
+int rc_motor_init_freq(int pwm_frequency_hz);
+
 /**
  * @brief      Puts all 4 motors into a free-spin (0 throttle) state, puts the
  *             h-bridges into standby mode, and closes all file pointers to GPIO

library/src/io/pwm.c

@@ -12,6 +12,7 @@
 #include <string.h>
 #include <errno.h>
 #include <rc/pwm.h>
+#include <rc/time.h>
 
 #define MIN_HZ 1
 #define MAX_HZ 1000000000
@@ -90,6 +91,42 @@ int __export_channels(int ss)
 	return 0;
 }
 
+/**
+ * @brief      unexports A and B pwm channels
+ *
+ * @param[in]  ss    subsystem, to export
+ *
+ * @return     0 on succcess, -1 on failure
+ */
+int __unexport_channels(int ss)
+{
+	int unexport_fd=0;
+	char buf[MAXBUF];
+	int len;
+
+	// open export file for that subsystem
+	len = snprintf(buf, sizeof(buf), BASE_DIR "%d/unexport", ss*2);
+	unexport_fd = open(buf, O_WRONLY);
+	if(unlikely(unexport_fd<0)){
+		perror("ERROR in rc_pwm_init, can't open pwm unexport file for writing");
+		fprintf(stderr,"Probably kernel or BeagleBone image is too old\n");
+		return -1;
+	}
+	len=write(unexport_fd, "0", 2);
+	if(unlikely(len<0 && errno!=EBUSY && errno!=ENODEV)){
+		perror("ERROR: in rc_pwm_init, failed to write 0 to unexport file");
+		return -1;
+	}
+	len=write(unexport_fd, "1", 2);
+	if(unlikely(len<0 && errno!=EBUSY  && errno!=ENODEV)){
+		perror("ERROR: in rc_pwm_init, failed to write 1 to unexport file");
+		return -1;
+	}
+	close(unexport_fd);
+	return 0;
+}
+
+
 int rc_pwm_init(int ss, int frequency)
 {
 	int periodA_fd; // pointers to frequency file descriptor
@@ -111,9 +148,17 @@ int rc_pwm_init(int ss, int frequency)
 		return -1;
 	}
 
-	// export channels first
+	// unexport then export channels first
+	if(__unexport_channels(ss)==-1) return -1;
 	if(__export_channels(ss)==-1) return -1;
 
+	// wait for udev to set correct permissions
+
+	//system("ls -la /sys/class/pwm/pwmchip4/pwm-4:0/");
+	//system("udevadm trigger");
+	//rc_usleep(1000000);
+	//system("ls -la /sys/class/pwm/pwmchip4/pwm-4:0/");
+
 	#ifdef DEBUG
 	printf("pwm ss:%d mode:%d\n",ss,mode);
 	#endif
@@ -122,11 +167,18 @@ int rc_pwm_init(int ss, int frequency)
 	if(mode==0)	len = snprintf(buf, sizeof(buf), BASE_DIR "%d/pwm0/duty_cycle", ss*2); // mode 0
 	else		len = snprintf(buf, sizeof(buf), BASE_DIR "%d/pwm-%d:0/duty_cycle", ss*2, ss*2); // mode 1
 	dutyA_fd[ss] = open(buf,O_WRONLY);
+
 	if(unlikely(dutyA_fd[ss]==-1)){
-		perror("ERROR in rc_pwm_init, failed to open duty_cycle channel A FD");
-		fprintf(stderr,"tried accessing: %s\n", buf);
-		return -1;
+		// first error is probably from udev being slow, wait a bit and try again
+		rc_usleep(600000);
+		dutyA_fd[ss] = open(buf,O_WRONLY);
+		if(unlikely(dutyA_fd[ss]==-1)){
+			perror("ERROR in rc_pwm_init, failed to open duty_cycle channel A FD");
+			fprintf(stderr,"tried accessing: %s\n", buf);
+			return -1;
+		}
 	}
+
 	if(mode==0)	len = snprintf(buf, sizeof(buf), BASE_DIR "%d/pwm1/duty_cycle", ss*2); // mode 0
 	else		len = snprintf(buf, sizeof(buf), BASE_DIR "%d/pwm-%d:1/duty_cycle", ss*2, ss*2); // mode 1
 	dutyB_fd[ss] = open(buf,O_WRONLY);
@@ -295,6 +347,12 @@ int rc_pwm_cleanup(int ss)
 	// close fds
 	close(enableA_fd);
 	close(enableB_fd);
+	close(dutyA_fd[ss]);
+	close(dutyB_fd[ss]);
+
+	// unexport channels, not critical if this fails since everything else
+	// has been closed
+	__unexport_channels(ss);
 
 	init_flag[ss] = 0;
 	return 0;

library/src/motor.c

@@ -41,7 +41,7 @@
 #define MOT_STBY		0,20	//gpio0.20	P9.41
 
 #define CHANNELS		4
-#define PWM_FREQ		25000	// 25kHz
+
 
 // polarity of the motor connections
 static const double polarity[]={1.0,-1.0,-1.0,1.0};
@@ -59,6 +59,12 @@ static int pwmch[CHANNELS];
 
 
 int rc_motor_init()
+{
+	return rc_motor_init_freq(RC_MOTOR_DEFAULT_PWM_FREQ);
+}
+
+
+int rc_motor_init_freq(int pwm_frequency_hz)
 {
 	int i;
 
@@ -108,11 +114,11 @@ int rc_motor_init()
 	pwmch[3]='B';
 
 	// set up pwm channels
-	if(unlikely(rc_pwm_init(1,PWM_FREQ))){
+	if(unlikely(rc_pwm_init(1,pwm_frequency_hz))){
 		fprintf(stderr,"ERROR in rc_motor_init, failed to initialize pwm subsystem 1\n");
 		return -1;
 	}
-	if(unlikely(rc_pwm_init(2,PWM_FREQ))){
+	if(unlikely(rc_pwm_init(2,pwm_frequency_hz))){
 		fprintf(stderr,"ERROR in rc_motor_init, failed to initialize pwm subsystem 2\n");
 		return -1;
 	}