diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt index 3409707ecd..017865fdb4 100644 --- a/src/drivers/pwm_out/CMakeLists.txt +++ b/src/drivers/pwm_out/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE drivers__pwm_out MAIN pwm_out COMPILE_FLAGS - -Wno-error=unused-variable SRCS PWMOut.cpp PWMOut.hpp diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index c86380ca7d..84c59b53a9 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -379,6 +379,11 @@ int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_ return OK; } +int PWMOut::set_i2c_bus_clock(unsigned bus, unsigned clock_hz) +{ + return device::I2C::set_bus_clock(bus, clock_hz); +} + void PWMOut::update_current_rate() { /* @@ -500,7 +505,6 @@ int PWMOut::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -#if defined(BOARD_HAS_CAPTURE) void PWMOut::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { @@ -511,10 +515,8 @@ void PWMOut::capture_trampoline(void *context, uint32_t chan_index, void PWMOut::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { - fprintf(stdout, "PWMOut: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, - overflow); + fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow); } -#endif // BOARD_HAS_CAPTURE void PWMOut::update_pwm_out_state(bool on) { @@ -601,11 +603,10 @@ void PWMOut::update_params() updateParams(); } -int PWMOut::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +int PWMOut::ioctl(file *filp, int cmd, unsigned long arg) { - int ret = -ENOTTY; + int ret; -#if defined(BOARD_HAS_CAPTURE) /* try it as a Capture ioctl next */ ret = capture_ioctl(filp, cmd, arg); @@ -613,8 +614,6 @@ int PWMOut::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) return ret; } -#endif // BOARD_HAS_CAPTURE - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { case MODE_1PWM: @@ -622,14 +621,11 @@ int PWMOut::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) case MODE_3PWM: case MODE_4PWM: case MODE_5PWM: -#if defined(BOARD_HAS_CAPTURE) case MODE_2PWM2CAP: case MODE_3PWM1CAP: case MODE_4PWM1CAP: case MODE_4PWM2CAP: case MODE_5PWM1CAP: -#endif // BOARD_HAS_CAPTURE - #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 case MODE_6PWM: #endif @@ -655,7 +651,7 @@ int PWMOut::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) return ret; } -int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; @@ -931,8 +927,6 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(11): case PWM_SERVO_SET(10): case PWM_SERVO_SET(9): - - // FALLTHROUGH case PWM_SERVO_SET(8): if (_mode < MODE_14PWM) { ret = -EINVAL; @@ -942,10 +936,9 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - // FALLTHROUGH case PWM_SERVO_SET(7): - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_SET(6): if (_mode < MODE_8PWM) { ret = -EINVAL; @@ -956,7 +949,7 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_SET(5): if (_mode < MODE_6PWM) { ret = -EINVAL; @@ -966,7 +959,7 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_SET(4): if (_mode < MODE_5PWM) { ret = -EINVAL; @@ -975,21 +968,21 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) #endif - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_SET(3): if (_mode < MODE_4PWM) { ret = -EINVAL; break; } - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_SET(2): if (_mode < MODE_3PWM) { ret = -EINVAL; break; } - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): if (arg <= 2100) { @@ -1015,10 +1008,9 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) } #endif - #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_GET(7): case PWM_SERVO_GET(6): if (_mode < MODE_8PWM) { @@ -1030,7 +1022,7 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_GET(5): if (_mode < MODE_6PWM) { ret = -EINVAL; @@ -1038,10 +1030,9 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) } #endif - #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_GET(4): if (_mode < MODE_5PWM) { ret = -EINVAL; @@ -1050,21 +1041,21 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) #endif - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_GET(3): if (_mode < MODE_4PWM) { ret = -EINVAL; break; } - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_GET(2): if (_mode < MODE_3PWM) { ret = -EINVAL; break; } - // FALLTHROUGH + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); @@ -1309,10 +1300,29 @@ int PWMOut::pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) return ret; } -#if defined(BOARD_HAS_CAPTURE) -int PWMOut::capture_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +void PWMOut::sensor_reset(int ms) { - int ret = -EINVAL; + if (ms < 1) { + ms = 1; + } + + board_spi_reset(ms, 0xffff); +} + +void PWMOut::peripheral_reset(int ms) +{ + if (ms < 1) { + ms = 10; + } + + board_peripheral_reset(ms); +} + +int PWMOut::capture_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + +#if defined(BOARD_HAS_CAPTURE) lock(); @@ -1453,9 +1463,11 @@ int PWMOut::capture_ioctl(cdev::file_t *filp, int cmd, unsigned long arg) unlock(); +#else + ret = -ENOTTY; +#endif return ret; } -#endif // BOARD_HAS_CAPTURE int PWMOut::fmu_new_mode(PortMode new_mode) { @@ -1463,7 +1475,9 @@ int PWMOut::fmu_new_mode(PortMode new_mode) return -1; } - PWMOut::Mode servo_mode = PWMOut::MODE_NONE; + PWMOut::Mode servo_mode; + + servo_mode = PWMOut::MODE_NONE; switch (new_mode) { case PORT_FULL_GPIO: @@ -1586,7 +1600,17 @@ int PWMOut::fmu_new_mode(PortMode new_mode) return OK; } -#if defined(BOARD_HAS_CAPTURE) + +namespace +{ + +int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz) +{ + return PWMOut::set_i2c_bus_clock(bus, clock_hz); +} + +} // namespace + int PWMOut::test() { int fd; @@ -1777,22 +1801,18 @@ err_out_no_test: ::close(fd); return rv; } -#endif // BOARD_HAS_CAPTURE int PWMOut::custom_command(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[0]; -#if defined(__PX4_NUTTX) - - /* does not operate on a PWMOut instance */ + /* does not operate on a FMU instance */ if (!strcmp(verb, "i2c")) { - // TODO: remove sensor reset from drivers/pwm_out if (argc > 2) { int bus = strtol(argv[1], 0, 0); int clock_hz = strtol(argv[2], 0, 0); - int ret = device::I2C::set_bus_clock(bus, clock_hz); + int ret = fmu_new_i2c_speed(bus, clock_hz); if (ret) { PX4_ERR("setting I2C clock failed"); @@ -1805,34 +1825,31 @@ int PWMOut::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "sensor_reset")) { - // TODO: remove sensor reset from drivers/pwm_out if (argc > 1) { int reset_time = strtol(argv[1], nullptr, 0); - board_spi_reset(math::max(reset_time, 1), 0xffff); + sensor_reset(reset_time); } else { + sensor_reset(0); + PX4_INFO("reset default time"); + } + + return 0; + } + + if (!strcmp(verb, "peripheral_reset")) { + if (argc > 2) { + int reset_time = strtol(argv[2], 0, 0); + peripheral_reset(reset_time); + + } else { + peripheral_reset(0); PX4_INFO("reset default time"); - // 1 millisecond - board_spi_reset(1, 0xffff); - } - } - - if (!strcmp(verb, "peripheral_reset")) { - // TODO: remove peripheral reset from drivers/pwm_out - if (argc > 2) { - int reset_time = strtol(argv[2], 0, 0); - // minimum 10 millisecond - board_peripheral_reset(math::max(10, reset_time)); - - } else { - PX4_INFO("reset default time"); - board_peripheral_reset(10); } return 0; } -#endif // __PX4_NUTTX /* start the FMU if not running */ if (!is_running()) { @@ -1920,14 +1937,10 @@ int PWMOut::custom_command(int argc, char *argv[]) return PWMOut::fmu_new_mode(new_mode); } -#if defined(BOARD_HAS_CAPTURE) - if (!strcmp(verb, "test")) { return test(); } -#endif // BOARD_HAS_CAPTURE - return print_usage("unknown command"); } @@ -1944,33 +1957,31 @@ int PWMOut::print_status() case MODE_2PWM: mode_str = "pwm2"; break; - case MODE_3PWM: mode_str = "pwm3"; break; - - case MODE_4PWM: mode_str = "pwm4"; break; - - case MODE_5PWM: mode_str = "pwm5"; break; - - case MODE_6PWM: mode_str = "pwm6"; break; - - case MODE_8PWM: mode_str = "pwm8"; break; -#if defined(BOARD_HAS_CAPTURE) - case MODE_2PWM2CAP: mode_str = "pwm2cap2"; break; + case MODE_3PWM: mode_str = "pwm3"; break; + case MODE_3PWM1CAP: mode_str = "pwm3cap1"; break; + case MODE_4PWM: mode_str = "pwm4"; break; + case MODE_4PWM1CAP: mode_str = "pwm4cap1"; break; case MODE_4PWM2CAP: mode_str = "pwm4cap2"; break; + case MODE_5PWM: mode_str = "pwm5"; break; + case MODE_5PWM1CAP: mode_str = "pwm5cap1"; break; + case MODE_6PWM: mode_str = "pwm6"; break; + + case MODE_8PWM: mode_str = "pwm8"; break; + case MODE_4CAP: mode_str = "cap4"; break; case MODE_5CAP: mode_str = "cap5"; break; case MODE_6CAP: mode_str = "cap6"; break; -#endif // BOARD_HAS_CAPTURE default: break; @@ -2031,27 +2042,24 @@ mixer files. PRINT_MODULE_USAGE_COMMAND("mode_gpio"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM"); #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - PRINT_MODULE_USAGE_COMMAND("mode_pwm8"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm8"); #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - PRINT_MODULE_USAGE_COMMAND("mode_pwm6"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm5"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm6"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm5"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1"); PRINT_MODULE_USAGE_COMMAND("mode_pwm4"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm3"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm3"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1"); PRINT_MODULE_USAGE_COMMAND("mode_pwm2"); -#if defined(BOARD_HAS_CAPTURE) - PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2"); -#endif // BOARD_HAS_CAPTURE + PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2"); #endif #if defined(BOARD_HAS_PWM) - PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); #endif -#if defined(__PX4_NUTTX) PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)"); PRINT_MODULE_USAGE_ARG("", "Delay time in ms between reset and re-enabling", true); PRINT_MODULE_USAGE_COMMAND_DESCR("peripheral_reset", "Reset board peripherals"); @@ -2059,7 +2067,6 @@ mixer files. PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "Configure I2C clock rate"); PRINT_MODULE_USAGE_ARG(" ", "Specify the bus id (>=0) and rate in Hz", false); -#endif // __PX4_NUTTX PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test inputs and outputs"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index df07483ea1..8b36b55918 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -36,12 +36,11 @@ #include #include -#include -#include -#include -#include - +#include +#include +#include #include +#include #include #include #include @@ -49,6 +48,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -59,14 +62,6 @@ #include #include -#if defined(__PX4_NUTTX) -#include -#endif // __PX4_NUTTX - -#if defined(BOARD_HAS_CAPTURE) -#include -#endif // BOARD_HAS_CAPTURE - using namespace time_literals; /** Mode given via CLI */ @@ -81,14 +76,12 @@ enum PortMode { PORT_PWM3, PORT_PWM2, PORT_PWM1, -#if defined(BOARD_HAS_CAPTURE) PORT_PWM3CAP1, PORT_PWM4CAP1, PORT_PWM4CAP2, PORT_PWM5CAP1, PORT_PWM2CAP2, PORT_CAPTURE, -#endif // BOARD_HAS_CAPTURE }; #if !defined(BOARD_HAS_PWM) @@ -140,20 +133,20 @@ public: /** change the FMU mode of the running module */ static int fmu_new_mode(PortMode new_mode); - virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); + static int test(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int init(); int set_mode(Mode mode); Mode get_mode() { return _mode; } -#if defined(BOARD_HAS_CAPTURE) - static int test(); + static int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); -#endif // BOARD_HAS_CAPTURE void update_pwm_trims(); @@ -176,6 +169,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + unsigned _num_outputs{0}; int _class_instance{-1}; @@ -188,18 +182,20 @@ private: perf_counter_t _cycle_perf; + void capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); void update_current_rate(); - int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); - int pwm_ioctl(cdev::file_t *filp, int cmd, unsigned long arg); + int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); void update_pwm_rev_mask(); void update_pwm_out_state(bool on); void update_params(); -#if defined(BOARD_HAS_CAPTURE) - void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + static void sensor_reset(int ms); + static void peripheral_reset(int ms); + int capture_ioctl(file *filp, int cmd, unsigned long arg); -#endif // BOARD_HAS_CAPTURE PWMOut(const PWMOut &) = delete; PWMOut operator=(const PWMOut &) = delete; diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index ab3b2fa4a1..a1ee318a48 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -311,11 +311,4 @@ private: } // namespace cdev -// class instance for primary driver of each class -enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY = 0, - CLASS_DEVICE_SECONDARY = 1, - CLASS_DEVICE_TERTIARY = 2 -}; - #endif /* _CDEV_HPP */ diff --git a/src/lib/drivers/device/CDev.hpp b/src/lib/drivers/device/CDev.hpp index 5cfe24e834..743e2b9fc6 100644 --- a/src/lib/drivers/device/CDev.hpp +++ b/src/lib/drivers/device/CDev.hpp @@ -90,4 +90,11 @@ public: } // namespace device +// class instance for primary driver of each class +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 +}; + #endif /* _DEVICE_CDEV_HPP */