diff --git a/examples/src/rc_test_motors.c b/examples/src/rc_test_motors.c index d9613b2d..8a98742b 100644 --- a/examples/src/rc_test_motors.c +++ b/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){ diff --git a/library/Makefile b/library/Makefile index 15ec34f7..a695656e 100644 --- a/library/Makefile +++ b/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 diff --git a/library/include/rc/motor.h b/library/include/rc/motor.h index 35ffef85..8de9cc8a 100644 --- a/library/include/rc/motor.h +++ b/library/include/rc/motor.h @@ -25,6 +25,8 @@ extern "C" { #endif +#define RC_MOTOR_DEFAULT_PWM_FREQ 25000 // 25kHz + /** * @brief Initializes all 4 motors and leaves them in a free-spin (0 @@ -32,13 +34,35 @@ extern "C" { * * 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 diff --git a/library/src/io/pwm.c b/library/src/io/pwm.c index f99bf955..2dd2c57e 100644 --- a/library/src/io/pwm.c +++ b/library/src/io/pwm.c @@ -12,6 +12,7 @@ #include #include #include +#include #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; diff --git a/library/src/motor.c b/library/src/motor.c index e15fd68f..6ce5118c 100644 --- a/library/src/motor.c +++ b/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; }