From 1913ae19f166d61038e436eff73efa42ac4d8f13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 10:45:30 +0200 Subject: add param for qnh --- src/drivers/ms5611/ms5611.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 10 ++++++++++ src/modules/sensors/sensors.cpp | 40 +++++++++++++++++++++++++++++-------- 3 files changed, 44 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 873fa62c4..889643d0d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -130,7 +130,7 @@ protected: float _T; /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ + unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; @@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) irqrestore(flags); return -ENOMEM; } - irqrestore(flags); + irqrestore(flags); return OK; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7ce6ef5ef..229bfe3ce 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); +/** + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensor Calibration + * @unit hPa + */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + /** * Board rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4e8a8c01d..d8e1fe9c8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -235,7 +235,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -258,7 +258,7 @@ private: int board_rotation; int external_mag_rotation; - + float board_offset[3]; int rc_map_roll; @@ -301,6 +301,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float baro_qnh; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -354,9 +356,11 @@ private: param_t board_rotation; param_t external_mag_rotation; - + param_t board_offset[3]; + param_t baro_qnh; + } _parameter_handles; /**< handles for interesting parameters */ @@ -611,12 +615,15 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); - + /* rotation offsets */ _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + /* fetch initial parameter values */ parameters_update(); } @@ -828,19 +835,36 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); - + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - + /** fine tune board offset on parameter update **/ - math::Matrix<3, 3> board_rotation_offset; + math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + _board_rotation = _board_rotation * board_rotation_offset; + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int fd; + fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + errx(1, "FATAL: no barometer found"); + + } else { + warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); + int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (ret) { + errx(ret, "qnh could not be set"); + } + } + close(fd); + return OK; } -- cgit v1.2.3 From 91d8cb2edcda1fde3c1765a659b2445592ed03f1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 13:45:51 +0200 Subject: sensors: move close to correct position --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d8e1fe9c8..0d51667d0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -862,8 +862,8 @@ Sensors::parameters_update() if (ret) { errx(ret, "qnh could not be set"); } + close(fd); } - close(fd); return OK; } -- cgit v1.2.3 From 86ae2e489fdd3656faa38202f44e4b53342e450b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 15:21:27 +0200 Subject: sensors: do not exit task on error --- src/modules/sensors/sensors.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0d51667d0..aac297ef8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -143,6 +143,12 @@ #define STICK_ON_OFF_LIMIT 0.75f +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + /** * Sensor app start / stop handling function * @@ -466,12 +472,6 @@ private: namespace sensors { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Sensors *g_sensors = nullptr; } @@ -860,7 +860,9 @@ Sensors::parameters_update() warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (ret) { - errx(ret, "qnh could not be set"); + warnx("qnh could not be set"); + close(fd); + return ERROR; } close(fd); } -- cgit v1.2.3 From 16694051c927b35a853c6ee41a00ad69c81f0bd0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 13:16:58 +0200 Subject: Require a digit ahead of the dot for a valid number for the SF0x output --- src/drivers/sf0x/sf0x.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index bca1715fa..80ecab2ee 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -598,7 +598,8 @@ SF0X::collect() memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } - if (_linebuf[i] == '.') { + /* we need a digit before the dot and a dot for a valid number */ + if (i > 0 && _linebuf[i] == '.') { valid = true; } } -- cgit v1.2.3 From 0d87dc992312526bb96269d706e2869d540926f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:50:13 +0200 Subject: mavlink: Handle command int packets --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 60 ++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 3 files changed, 63 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c27716f74..976b6d4d5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1655,6 +1655,8 @@ Mavlink::display_status() printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } + printf("\tmavlink chan: #%u\n", _channel); + if (_rstatus.timestamp > 0) { printf("\ttype:\t\t"); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e9a9ff133..a602344fd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -136,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW: handle_message_optical_flow(msg); break; @@ -275,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + void MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3999b8b01..91125736c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,6 +110,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); -- cgit v1.2.3 From 690843c53aa03283688e04fbe2894fc986f31b6b Mon Sep 17 00:00:00 2001 From: muharred Date: Wed, 20 Aug 2014 13:06:39 +0300 Subject: Reset excitement mode for hmc5883 sensor before applying a new one to avoid getting reserved 11 bits set in case of a set_excitement(1) call following a set_excitement(-1) call. --- src/drivers/hmc5883/hmc5883.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0e9a961ac..e4ecfa6b5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable) if (OK != ret) perf_count(_comms_errors); + _conf_reg &= ~0x03; // reset previous excitement mode if (((int)enable) < 0) { _conf_reg |= 0x01; } else if (enable > 0) { _conf_reg |= 0x02; - } else { - _conf_reg &= ~0x03; } // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); -- cgit v1.2.3 From bb1a45ac42001d96cbc4a4a4fbe69c989c261094 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 15:22:15 +0200 Subject: skeleton for adafruiti2cpwm driver --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 362 +++++++++++++++++++++ ...o_Adafruit_PWM_Servo_Driver_Library_license.txt | 29 ++ src/drivers/adafruiti2cpwm/module.mk | 41 +++ 3 files changed, 432 insertions(+) create mode 100644 src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp create mode 100644 src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt create mode 100644 src/drivers/adafruiti2cpwm/module.mk (limited to 'src') diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp new file mode 100644 index 000000000..959b90db8 --- /dev/null +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -0,0 +1,362 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file adafruiti2cpwm.cpp + * + * Driver for the adafruit I2C PWM converter based on the PCA9685 + * https://www.adafruit.com/product/815 + * + * some code is adapted from the arduino library for the board + * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library + * see the arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors + * + * @author Thomas Gubler + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OFF_H 0xFD + +#define ADDR PCA9685_SUBADR1 ///< I2C adress + +#define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" +#define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION + +class ADAFRUITI2CPWM : public device::I2C +{ +public: + ADAFRUITI2CPWM(int bus=ADAFRUITI2CPWM_BUS, uint8_t address=ADDR); + virtual ~ADAFRUITI2CPWM(); + + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } + +private: + work_s _work; + + + enum IOX_MODE _mode; + bool _running; + int _i2cpwm_interval; + bool _should_run; + + static void i2cpwm_trampoline(void *arg); + void i2cpwm(); + +}; + +/* for now, we only support one board */ +namespace +{ +ADAFRUITI2CPWM *g_adafruiti2cpwm; +} + +void adafruiti2cpwm_usage(); + +extern "C" __EXPORT int adafruiti2cpwm_main(int argc, char *argv[]); + +ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : + I2C("adafruiti2cpwm", ADAFRUITI2CPWM_DEVICE_PATH, bus, address, 100000), + _mode(IOX_MODE_OFF), + _running(false), + _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _should_run(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +ADAFRUITI2CPWM::~ADAFRUITI2CPWM() +{ +} + +int +ADAFRUITI2CPWM::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + switch (cmd) { + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + case IOX_MODE_ON: + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + } + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +void +ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) +{ + ADAFRUITI2CPWM *i2cpwm = reinterpret_cast(arg); + + i2cpwm->i2cpwm(); +} + +/** + * Main loop function + */ +void +ADAFRUITI2CPWM::i2cpwm() +{ + + if (_mode == IOX_MODE_TEST_OUT) { + + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _should_run = false; + } else { + + } + + + // check if any activity remains, else stop + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); +} + +void +print_usage() +{ + warnx("missing command: try 'start', 'test', 'stop'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); + warnx(" -a addr (0x%x)", ADDR); +} + +int +adafruiti2cpwm_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2caddr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2caddr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + print_usage(); + exit(0); + } + } + + if (optind >= argc) { + print_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_adafruiti2cpwm != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_adafruiti2cpwm = new ADAFRUITI2CPWM(PX4_I2C_BUS_EXPANSION, i2caddr); + + if (g_adafruiti2cpwm != nullptr && OK != g_adafruiti2cpwm->init()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm = nullptr; + } + + if (g_adafruiti2cpwm == nullptr) { + errx(1, "init failed"); + } + } + + if (g_adafruiti2cpwm == nullptr) { + g_adafruiti2cpwm = new ADAFRUITI2CPWM(i2cdevice, i2caddr); + + if (g_adafruiti2cpwm == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_adafruiti2cpwm->init()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + // need the driver past this point + if (g_adafruiti2cpwm == nullptr) { + warnx("not started, run adafruiti2cpwm start"); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_adafruiti2cpwm->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_adafruiti2cpwm->is_running()) { + delete g_adafruiti2cpwm; + g_adafruiti2cpwm= nullptr; + warnx("stopped, exiting"); + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + print_usage(); + exit(0); +} diff --git a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt new file mode 100644 index 000000000..9c5eb42eb --- /dev/null +++ b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt @@ -0,0 +1,29 @@ +The following license agreement covers re-used code from the arduino driver +for the Adafruit I2C to PWM converter. + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/drivers/adafruiti2cpwm/module.mk b/src/drivers/adafruiti2cpwm/module.mk new file mode 100644 index 000000000..1f24591cf --- /dev/null +++ b/src/drivers/adafruiti2cpwm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Driver for the adafruit I2C PWM converter, +# which allow to control servos via I2C. +# https://www.adafruit.com/product/815 + +MODULE_COMMAND = adafruiti2cpwm + +SRCS = adafruiti2cpwm.cpp -- cgit v1.2.3 From 66991f9bb1493437a05ef4b4a704b43889a3413b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 11:19:18 +0200 Subject: bring up adafruit i2c pwm driver includes the test function, fucntionality verified with scope and servo --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 267 ++++++++++++++++++++++++-- 1 file changed, 253 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp index 959b90db8..6fc4f21aa 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -37,9 +37,10 @@ * Driver for the adafruit I2C PWM converter based on the PCA9685 * https://www.adafruit.com/product/815 * - * some code is adapted from the arduino library for the board + * Parts of the code are adapted from the arduino library for the board * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library - * see the arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * for the license of these parts see the + * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors * * @author Thomas Gubler @@ -84,12 +85,25 @@ #define ALLLED_ON_L 0xFA #define ALLLED_ON_H 0xFB #define ALLLED_OFF_L 0xFC -#define ALLLED_OFF_H 0xFD +#define ALLLED_OF -#define ADDR PCA9685_SUBADR1 ///< I2C adress +#define ADDR 0x40 // I2C adress #define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" #define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION +#define ADAFRUITI2CPWM_PWMFREQ 60.0f + +#define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f + +#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2 +#define ADAFRUITI2CPWM_SCALE ADAFRUITI2CPWM_CENTER + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; class ADAFRUITI2CPWM : public device::I2C { @@ -100,6 +114,8 @@ public: virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int info(); + virtual int reset(); bool is_running() { return _running; } private: @@ -110,10 +126,40 @@ private: bool _running; int _i2cpwm_interval; bool _should_run; + perf_counter_t _comms_errors; + + uint8_t _msg[6]; static void i2cpwm_trampoline(void *arg); void i2cpwm(); + /** + * Helper function to set the pwm frequency + */ + int setPWMFreq(float freq); + + /** + * Helper function to set the demanded pwm value + * @param num pwm output number + */ + int setPWM(uint8_t num, uint16_t on, uint16_t off); + + /** + * Sets pin without having to deal with on/off tick placement and properly handles + * a zero value as completely off. Optional invert parameter supports inverting + * the pulse for sinking to ground. + * @param num pwm output number + * @param val should be a value from 0 to 4095 inclusive. + */ + int setPin(uint8_t num, uint16_t val, bool invert = false); + + + /* Wrapper to read a byte from addr */ + int read8(uint8_t addr, uint8_t &value); + + /* Wrapper to wite a byte to addr */ + int write8(uint8_t addr, uint8_t value); + }; /* for now, we only support one board */ @@ -131,9 +177,11 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : _mode(IOX_MODE_OFF), _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), - _should_run(false) + _should_run(false), + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")) { memset(&_work, 0, sizeof(_work)); + memset(_msg, 0, sizeof(_msg)); } ADAFRUITI2CPWM::~ADAFRUITI2CPWM() @@ -145,12 +193,18 @@ ADAFRUITI2CPWM::init() { int ret; ret = I2C::init(); + if (ret != OK) { + return ret; + } + ret = reset(); if (ret != OK) { return ret; } - return OK; + ret = setPWMFreq(ADAFRUITI2CPWM_PWMFREQ); + + return ret; } int @@ -165,8 +219,13 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) switch ((IOX_MODE)arg) { case IOX_MODE_OFF: + warnx("shutting down"); + break; case IOX_MODE_ON: + warnx("starting"); + break; case IOX_MODE_TEST_OUT: + warnx("test starting"); break; default: @@ -176,6 +235,13 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) _mode = (IOX_MODE)arg; } + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, 1); + } + + return OK; default: @@ -187,6 +253,20 @@ ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } +int +ADAFRUITI2CPWM::info() +{ + int ret = OK; + + if (is_running()) { + warnx("Driver is running, mode: %u", _mode); + } else { + warnx("Driver started but not running"); + } + + return ret; +} + void ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) { @@ -201,17 +281,16 @@ ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) void ADAFRUITI2CPWM::i2cpwm() { - if (_mode == IOX_MODE_TEST_OUT) { - + setPin(0, ADAFRUITI2CPWM_CENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { + _should_run = true; } - // check if any activity remains, else stop if (!_should_run) { _running = false; @@ -223,10 +302,152 @@ ADAFRUITI2CPWM::i2cpwm() work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); } +int +ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) +{ + int ret; + /* convert to correct message */ + _msg[0] = LED0_ON_L + 4 * num; + _msg[1] = on; + _msg[2] = on >> 8; + _msg[3] = off; + _msg[4] = off >> 8; + + /* try i2c transfer */ + ret = transfer(_msg, 5, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + + return ret; +} + +int +ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + if (val > 4095) { + val = 4095; + } + if (invert) { + if (val == 0) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, 4095-val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, val); + } + } + + return ERROR; +} + +int +ADAFRUITI2CPWM::setPWMFreq(float freq) +{ + int ret = OK; + freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue + https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() + uint8_t oldmode; + ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + + usleep(5000); //5ms delay (from arduino driver) + + ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { + return ret; + } + + return ret; +} + +/* Wrapper to read a byte from addr */ +int +ADAFRUITI2CPWM::read8(uint8_t addr, uint8_t &value) +{ + int ret = OK; + + /* send addr */ + ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { + goto fail_read; + } + + /* get value */ + ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { + goto fail_read; + } + + return ret; + +fail_read: + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + + return ret; +} + +int ADAFRUITI2CPWM::reset(void) { + warnx("resetting"); + return write8(PCA9685_MODE1, 0x0); +} + +/* Wrapper to wite a byte to addr */ +int +ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { + int ret = OK; + _msg[0] = addr; + _msg[1] = value; + /* send addr and value */ + ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + return ret; +} + void -print_usage() +adafruiti2cpwm_usage() { - warnx("missing command: try 'start', 'test', 'stop'"); + warnx("missing command: try 'start', 'test', 'stop', 'info'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); warnx(" -a addr (0x%x)", ADDR); @@ -252,13 +473,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) break; default: - print_usage(); + adafruiti2cpwm_usage(); exit(0); } } if (optind >= argc) { - print_usage(); + adafruiti2cpwm_usage(); exit(1); } @@ -300,6 +521,13 @@ adafruiti2cpwm_main(int argc, char *argv[]) errx(1, "init failed"); } } + fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); + } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); + close(fd); + exit(0); } @@ -310,6 +538,17 @@ adafruiti2cpwm_main(int argc, char *argv[]) exit(1); } + if (!strcmp(verb, "info")) { + g_adafruiti2cpwm->info(); + exit(0); + } + + if (!strcmp(verb, "reset")) { + g_adafruiti2cpwm->reset(); + exit(0); + } + + if (!strcmp(verb, "test")) { fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); @@ -357,6 +596,6 @@ adafruiti2cpwm_main(int argc, char *argv[]) } } - print_usage(); + adafruiti2cpwm_usage(); exit(0); } -- cgit v1.2.3 From dd85c0407cbf552c7044d425d81ed346997e8572 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 15:04:24 +0200 Subject: adafruit i2c pwm: listen to uorb for setpoints --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 52 +++++++++++++++++++++++++-- 1 file changed, 49 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp index 6fc4f21aa..6d3359999 100644 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,9 @@ #include #include +#include +#include + #include #include @@ -92,12 +96,15 @@ #define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" #define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION #define ADAFRUITI2CPWM_PWMFREQ 60.0f +#define ADAFRUITI2CPWM_NCHANS 16 // total amount of pwm outputs #define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) #define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2 -#define ADAFRUITI2CPWM_SCALE ADAFRUITI2CPWM_CENTER +#define ADAFRUITI2CPWM_HALFRANGE ((ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2) +#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMIN + ADAFRUITI2CPWM_HALFRANGE) +#define ADAFRUITI2CPWM_MAXSERVODEG 45 //maximal defelction in degrees +#define ADAFRUITI2CPWM_SCALE (ADAFRUITI2CPWM_HALFRANGE / (M_DEG_TO_RAD_F * ADAFRUITI2CPWM_MAXSERVODEG)) /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,6 +137,13 @@ private: uint8_t _msg[6]; + int _actuator_controls_sub; + struct actuator_controls_s _actuator_controls; + uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output + values as sent to the setPin() */ + + bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ + static void i2cpwm_trampoline(void *arg); void i2cpwm(); @@ -178,10 +192,14 @@ ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : _running(false), _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), _should_run(false), - _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")) + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), + _actuator_controls_sub(-1), + _actuator_controls(), + _mode_on_initialized(false) { memset(&_work, 0, sizeof(_work)); memset(_msg, 0, sizeof(_msg)); + memset(_current_values, 0, sizeof(_current_values)); } ADAFRUITI2CPWM::~ADAFRUITI2CPWM() @@ -287,7 +305,35 @@ ADAFRUITI2CPWM::i2cpwm() } else if (_mode == IOX_MODE_OFF) { _should_run = false; } else { + if (!_mode_on_initialized) { + /* Subscribe to actuator control 2 (payload group for gimbal) */ + _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); + /* set the uorb update interval lower than the driver pwm interval */ + orb_set_interval(_actuator_controls_sub, 1000.0f / ADAFRUITI2CPWM_PWMFREQ - 5); + _mode_on_initialized = true; + } + + /* Read the servo setpoints from the actuator control topics (gimbal) */ + bool updated; + orb_check(_actuator_controls_sub, &updated); + if (updated) { + orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + uint16_t new_value = ADAFRUITI2CPWM_CENTER + + (_actuator_controls.control[i] * ADAFRUITI2CPWM_SCALE); + debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && + isfinite(new_value) && + new_value >= 0 && + new_value <= ADAFRUITI2CPWM_SERVOMAX) { + /* This value was updated, send the command to adjust the PWM value */ + setPin(i, new_value); + _current_values[i] = new_value; + } + } + } _should_run = true; } -- cgit v1.2.3 From c414417cf8801f690582876cd723eca7273cbe6b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 16:21:02 +0200 Subject: rename adafruiti2cpwm to pca9685 --- src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp | 647 --------------------- ...o_Adafruit_PWM_Servo_Driver_Library_license.txt | 29 - src/drivers/adafruiti2cpwm/module.mk | 41 -- ...o_Adafruit_PWM_Servo_Driver_Library_license.txt | 29 + src/drivers/pca9685/module.mk | 42 ++ src/drivers/pca9685/pca9685.cpp | 647 +++++++++++++++++++++ 6 files changed, 718 insertions(+), 717 deletions(-) delete mode 100644 src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp delete mode 100644 src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt delete mode 100644 src/drivers/adafruiti2cpwm/module.mk create mode 100644 src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt create mode 100644 src/drivers/pca9685/module.mk create mode 100644 src/drivers/pca9685/pca9685.cpp (limited to 'src') diff --git a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp b/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp deleted file mode 100644 index 6d3359999..000000000 --- a/src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp +++ /dev/null @@ -1,647 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file adafruiti2cpwm.cpp - * - * Driver for the adafruit I2C PWM converter based on the PCA9685 - * https://www.adafruit.com/product/815 - * - * Parts of the code are adapted from the arduino library for the board - * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library - * for the license of these parts see the - * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file - * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors - * - * @author Thomas Gubler - */ - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#define PCA9685_SUBADR1 0x2 -#define PCA9685_SUBADR2 0x3 -#define PCA9685_SUBADR3 0x4 - -#define PCA9685_MODE1 0x0 -#define PCA9685_PRESCALE 0xFE - -#define LED0_ON_L 0x6 -#define LED0_ON_H 0x7 -#define LED0_OFF_L 0x8 -#define LED0_OFF_H 0x9 - -#define ALLLED_ON_L 0xFA -#define ALLLED_ON_H 0xFB -#define ALLLED_OFF_L 0xFC -#define ALLLED_OF - -#define ADDR 0x40 // I2C adress - -#define ADAFRUITI2CPWM_DEVICE_PATH "/dev/adafruiti2cpwm" -#define ADAFRUITI2CPWM_BUS PX4_I2C_BUS_EXPANSION -#define ADAFRUITI2CPWM_PWMFREQ 60.0f -#define ADAFRUITI2CPWM_NCHANS 16 // total amount of pwm outputs - -#define ADAFRUITI2CPWM_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) -#define ADAFRUITI2CPWM_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f - -#define ADAFRUITI2CPWM_HALFRANGE ((ADAFRUITI2CPWM_SERVOMAX - ADAFRUITI2CPWM_SERVOMIN)/2) -#define ADAFRUITI2CPWM_CENTER (ADAFRUITI2CPWM_SERVOMIN + ADAFRUITI2CPWM_HALFRANGE) -#define ADAFRUITI2CPWM_MAXSERVODEG 45 //maximal defelction in degrees -#define ADAFRUITI2CPWM_SCALE (ADAFRUITI2CPWM_HALFRANGE / (M_DEG_TO_RAD_F * ADAFRUITI2CPWM_MAXSERVODEG)) - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -class ADAFRUITI2CPWM : public device::I2C -{ -public: - ADAFRUITI2CPWM(int bus=ADAFRUITI2CPWM_BUS, uint8_t address=ADDR); - virtual ~ADAFRUITI2CPWM(); - - - virtual int init(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - virtual int info(); - virtual int reset(); - bool is_running() { return _running; } - -private: - work_s _work; - - - enum IOX_MODE _mode; - bool _running; - int _i2cpwm_interval; - bool _should_run; - perf_counter_t _comms_errors; - - uint8_t _msg[6]; - - int _actuator_controls_sub; - struct actuator_controls_s _actuator_controls; - uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output - values as sent to the setPin() */ - - bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ - - static void i2cpwm_trampoline(void *arg); - void i2cpwm(); - - /** - * Helper function to set the pwm frequency - */ - int setPWMFreq(float freq); - - /** - * Helper function to set the demanded pwm value - * @param num pwm output number - */ - int setPWM(uint8_t num, uint16_t on, uint16_t off); - - /** - * Sets pin without having to deal with on/off tick placement and properly handles - * a zero value as completely off. Optional invert parameter supports inverting - * the pulse for sinking to ground. - * @param num pwm output number - * @param val should be a value from 0 to 4095 inclusive. - */ - int setPin(uint8_t num, uint16_t val, bool invert = false); - - - /* Wrapper to read a byte from addr */ - int read8(uint8_t addr, uint8_t &value); - - /* Wrapper to wite a byte to addr */ - int write8(uint8_t addr, uint8_t value); - -}; - -/* for now, we only support one board */ -namespace -{ -ADAFRUITI2CPWM *g_adafruiti2cpwm; -} - -void adafruiti2cpwm_usage(); - -extern "C" __EXPORT int adafruiti2cpwm_main(int argc, char *argv[]); - -ADAFRUITI2CPWM::ADAFRUITI2CPWM(int bus, uint8_t address) : - I2C("adafruiti2cpwm", ADAFRUITI2CPWM_DEVICE_PATH, bus, address, 100000), - _mode(IOX_MODE_OFF), - _running(false), - _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), - _should_run(false), - _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), - _actuator_controls_sub(-1), - _actuator_controls(), - _mode_on_initialized(false) -{ - memset(&_work, 0, sizeof(_work)); - memset(_msg, 0, sizeof(_msg)); - memset(_current_values, 0, sizeof(_current_values)); -} - -ADAFRUITI2CPWM::~ADAFRUITI2CPWM() -{ -} - -int -ADAFRUITI2CPWM::init() -{ - int ret; - ret = I2C::init(); - if (ret != OK) { - return ret; - } - - ret = reset(); - if (ret != OK) { - return ret; - } - - ret = setPWMFreq(ADAFRUITI2CPWM_PWMFREQ); - - return ret; -} - -int -ADAFRUITI2CPWM::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = -EINVAL; - switch (cmd) { - - case IOX_SET_MODE: - - if (_mode != (IOX_MODE)arg) { - - switch ((IOX_MODE)arg) { - case IOX_MODE_OFF: - warnx("shutting down"); - break; - case IOX_MODE_ON: - warnx("starting"); - break; - case IOX_MODE_TEST_OUT: - warnx("test starting"); - break; - - default: - return -1; - } - - _mode = (IOX_MODE)arg; - } - - // if not active, kick it - if (!_running) { - _running = true; - work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, 1); - } - - - return OK; - - default: - // see if the parent class can make any use of it - ret = CDev::ioctl(filp, cmd, arg); - break; - } - - return ret; -} - -int -ADAFRUITI2CPWM::info() -{ - int ret = OK; - - if (is_running()) { - warnx("Driver is running, mode: %u", _mode); - } else { - warnx("Driver started but not running"); - } - - return ret; -} - -void -ADAFRUITI2CPWM::i2cpwm_trampoline(void *arg) -{ - ADAFRUITI2CPWM *i2cpwm = reinterpret_cast(arg); - - i2cpwm->i2cpwm(); -} - -/** - * Main loop function - */ -void -ADAFRUITI2CPWM::i2cpwm() -{ - if (_mode == IOX_MODE_TEST_OUT) { - setPin(0, ADAFRUITI2CPWM_CENTER); - _should_run = true; - } else if (_mode == IOX_MODE_OFF) { - _should_run = false; - } else { - if (!_mode_on_initialized) { - /* Subscribe to actuator control 2 (payload group for gimbal) */ - _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); - /* set the uorb update interval lower than the driver pwm interval */ - orb_set_interval(_actuator_controls_sub, 1000.0f / ADAFRUITI2CPWM_PWMFREQ - 5); - - _mode_on_initialized = true; - } - - /* Read the servo setpoints from the actuator control topics (gimbal) */ - bool updated; - orb_check(_actuator_controls_sub, &updated); - if (updated) { - orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); - for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - uint16_t new_value = ADAFRUITI2CPWM_CENTER + - (_actuator_controls.control[i] * ADAFRUITI2CPWM_SCALE); - debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, - (double)_actuator_controls.control[i]); - if (new_value != _current_values[i] && - isfinite(new_value) && - new_value >= 0 && - new_value <= ADAFRUITI2CPWM_SERVOMAX) { - /* This value was updated, send the command to adjust the PWM value */ - setPin(i, new_value); - _current_values[i] = new_value; - } - } - } - _should_run = true; - } - - // check if any activity remains, else stop - if (!_should_run) { - _running = false; - return; - } - - // re-queue ourselves to run again later - _running = true; - work_queue(LPWORK, &_work, (worker_t)&ADAFRUITI2CPWM::i2cpwm_trampoline, this, _i2cpwm_interval); -} - -int -ADAFRUITI2CPWM::setPWM(uint8_t num, uint16_t on, uint16_t off) -{ - int ret; - /* convert to correct message */ - _msg[0] = LED0_ON_L + 4 * num; - _msg[1] = on; - _msg[2] = on >> 8; - _msg[3] = off; - _msg[4] = off >> 8; - - /* try i2c transfer */ - ret = transfer(_msg, 5, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - } - - return ret; -} - -int -ADAFRUITI2CPWM::setPin(uint8_t num, uint16_t val, bool invert) -{ - // Clamp value between 0 and 4095 inclusive. - if (val > 4095) { - val = 4095; - } - if (invert) { - if (val == 0) { - // Special value for signal fully on. - return setPWM(num, 4096, 0); - } else if (val == 4095) { - // Special value for signal fully off. - return setPWM(num, 0, 4096); - } else { - return setPWM(num, 0, 4095-val); - } - } else { - if (val == 4095) { - // Special value for signal fully on. - return setPWM(num, 4096, 0); - } else if (val == 0) { - // Special value for signal fully off. - return setPWM(num, 0, 4096); - } else { - return setPWM(num, 0, val); - } - } - - return ERROR; -} - -int -ADAFRUITI2CPWM::setPWMFreq(float freq) -{ - int ret = OK; - freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue - https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ - float prescaleval = 25000000; - prescaleval /= 4096; - prescaleval /= freq; - prescaleval -= 1; - uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() - uint8_t oldmode; - ret = read8(PCA9685_MODE1, oldmode); - if (ret != OK) { - return ret; - } - uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep - - ret = write8(PCA9685_MODE1, newmode); // go to sleep - if (ret != OK) { - return ret; - } - ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler - if (ret != OK) { - return ret; - } - ret = write8(PCA9685_MODE1, oldmode); - if (ret != OK) { - return ret; - } - - usleep(5000); //5ms delay (from arduino driver) - - ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. - if (ret != OK) { - return ret; - } - - return ret; -} - -/* Wrapper to read a byte from addr */ -int -ADAFRUITI2CPWM::read8(uint8_t addr, uint8_t &value) -{ - int ret = OK; - - /* send addr */ - ret = transfer(&addr, sizeof(addr), nullptr, 0); - if (ret != OK) { - goto fail_read; - } - - /* get value */ - ret = transfer(nullptr, 0, &value, 1); - if (ret != OK) { - goto fail_read; - } - - return ret; - -fail_read: - perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - - return ret; -} - -int ADAFRUITI2CPWM::reset(void) { - warnx("resetting"); - return write8(PCA9685_MODE1, 0x0); -} - -/* Wrapper to wite a byte to addr */ -int -ADAFRUITI2CPWM::write8(uint8_t addr, uint8_t value) { - int ret = OK; - _msg[0] = addr; - _msg[1] = value; - /* send addr and value */ - ret = transfer(_msg, 2, nullptr, 0); - if (ret != OK) { - perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - } - return ret; -} - -void -adafruiti2cpwm_usage() -{ - warnx("missing command: try 'start', 'test', 'stop', 'info'"); - warnx("options:"); - warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); - warnx(" -a addr (0x%x)", ADDR); -} - -int -adafruiti2cpwm_main(int argc, char *argv[]) -{ - int i2cdevice = -1; - int i2caddr = ADDR; // 7bit - - int ch; - - // jump over start/off/etc and look at options first - while ((ch = getopt(argc, argv, "a:b:")) != EOF) { - switch (ch) { - case 'a': - i2caddr = strtol(optarg, NULL, 0); - break; - - case 'b': - i2cdevice = strtol(optarg, NULL, 0); - break; - - default: - adafruiti2cpwm_usage(); - exit(0); - } - } - - if (optind >= argc) { - adafruiti2cpwm_usage(); - exit(1); - } - - const char *verb = argv[optind]; - - int fd; - int ret; - - if (!strcmp(verb, "start")) { - if (g_adafruiti2cpwm != nullptr) { - errx(1, "already started"); - } - - if (i2cdevice == -1) { - // try the external bus first - i2cdevice = PX4_I2C_BUS_EXPANSION; - g_adafruiti2cpwm = new ADAFRUITI2CPWM(PX4_I2C_BUS_EXPANSION, i2caddr); - - if (g_adafruiti2cpwm != nullptr && OK != g_adafruiti2cpwm->init()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm = nullptr; - } - - if (g_adafruiti2cpwm == nullptr) { - errx(1, "init failed"); - } - } - - if (g_adafruiti2cpwm == nullptr) { - g_adafruiti2cpwm = new ADAFRUITI2CPWM(i2cdevice, i2caddr); - - if (g_adafruiti2cpwm == nullptr) { - errx(1, "new failed"); - } - - if (OK != g_adafruiti2cpwm->init()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm = nullptr; - errx(1, "init failed"); - } - } - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); - if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); - } - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); - close(fd); - - - exit(0); - } - - // need the driver past this point - if (g_adafruiti2cpwm == nullptr) { - warnx("not started, run adafruiti2cpwm start"); - exit(1); - } - - if (!strcmp(verb, "info")) { - g_adafruiti2cpwm->info(); - exit(0); - } - - if (!strcmp(verb, "reset")) { - g_adafruiti2cpwm->reset(); - exit(0); - } - - - if (!strcmp(verb, "test")) { - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); - - close(fd); - exit(ret); - } - - if (!strcmp(verb, "stop")) { - fd = open(ADAFRUITI2CPWM_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " ADAFRUITI2CPWM_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); - close(fd); - - // wait until we're not running any more - for (unsigned i = 0; i < 15; i++) { - if (!g_adafruiti2cpwm->is_running()) { - break; - } - - usleep(50000); - printf("."); - fflush(stdout); - } - printf("\n"); - fflush(stdout); - - if (!g_adafruiti2cpwm->is_running()) { - delete g_adafruiti2cpwm; - g_adafruiti2cpwm= nullptr; - warnx("stopped, exiting"); - exit(0); - } else { - warnx("stop failed."); - exit(1); - } - } - - adafruiti2cpwm_usage(); - exit(0); -} diff --git a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt deleted file mode 100644 index 9c5eb42eb..000000000 --- a/src/drivers/adafruiti2cpwm/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt +++ /dev/null @@ -1,29 +0,0 @@ -The following license agreement covers re-used code from the arduino driver -for the Adafruit I2C to PWM converter. - -Software License Agreement (BSD License) - -Copyright (c) 2012, Adafruit Industries -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: -1. Redistributions of source code must retain the above copyright -notice, this list of conditions and the following disclaimer. -2. Redistributions in binary form must reproduce the above copyright -notice, this list of conditions and the following disclaimer in the -documentation and/or other materials provided with the distribution. -3. Neither the name of the copyright holders nor the -names of its contributors may be used to endorse or promote products -derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/drivers/adafruiti2cpwm/module.mk b/src/drivers/adafruiti2cpwm/module.mk deleted file mode 100644 index 1f24591cf..000000000 --- a/src/drivers/adafruiti2cpwm/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Driver for the adafruit I2C PWM converter, -# which allow to control servos via I2C. -# https://www.adafruit.com/product/815 - -MODULE_COMMAND = adafruiti2cpwm - -SRCS = adafruiti2cpwm.cpp diff --git a/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt new file mode 100644 index 000000000..9c5eb42eb --- /dev/null +++ b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt @@ -0,0 +1,29 @@ +The following license agreement covers re-used code from the arduino driver +for the Adafruit I2C to PWM converter. + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/drivers/pca9685/module.mk b/src/drivers/pca9685/module.mk new file mode 100644 index 000000000..7a5c7996e --- /dev/null +++ b/src/drivers/pca9685/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Driver for the PCA9685 I2C PWM controller +# The chip is used on the adafruit I2C PWM converter, +# which allows to control servos via I2C. +# https://www.adafruit.com/product/815 + +MODULE_COMMAND = pca9685 + +SRCS = pca9685.cpp diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp new file mode 100644 index 000000000..c97d1e9ab --- /dev/null +++ b/src/drivers/pca9685/pca9685.cpp @@ -0,0 +1,647 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pca9685.cpp + * + * Driver for the PCA9685 I2C PWM module + * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815 + * + * Parts of the code are adapted from the arduino library for the board + * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library + * for the license of these parts see the + * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors + * + * @author Thomas Gubler + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OF + +#define ADDR 0x40 // I2C adress + +#define PCA9685_DEVICE_PATH "/dev/pca9685" +#define PCA9685_BUS PX4_I2C_BUS_EXPANSION +#define PCA9685_PWMFREQ 60.0f +#define PCA9685_NCHANS 16 // total amount of pwm outputs + +#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f + +#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2) +#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE) +#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees +#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +class PCA9685 : public device::I2C +{ +public: + PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + virtual ~PCA9685(); + + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int info(); + virtual int reset(); + bool is_running() { return _running; } + +private: + work_s _work; + + + enum IOX_MODE _mode; + bool _running; + int _i2cpwm_interval; + bool _should_run; + perf_counter_t _comms_errors; + + uint8_t _msg[6]; + + int _actuator_controls_sub; + struct actuator_controls_s _actuator_controls; + uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output + values as sent to the setPin() */ + + bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ + + static void i2cpwm_trampoline(void *arg); + void i2cpwm(); + + /** + * Helper function to set the pwm frequency + */ + int setPWMFreq(float freq); + + /** + * Helper function to set the demanded pwm value + * @param num pwm output number + */ + int setPWM(uint8_t num, uint16_t on, uint16_t off); + + /** + * Sets pin without having to deal with on/off tick placement and properly handles + * a zero value as completely off. Optional invert parameter supports inverting + * the pulse for sinking to ground. + * @param num pwm output number + * @param val should be a value from 0 to 4095 inclusive. + */ + int setPin(uint8_t num, uint16_t val, bool invert = false); + + + /* Wrapper to read a byte from addr */ + int read8(uint8_t addr, uint8_t &value); + + /* Wrapper to wite a byte to addr */ + int write8(uint8_t addr, uint8_t value); + +}; + +/* for now, we only support one board */ +namespace +{ +PCA9685 *g_pca9685; +} + +void pca9685_usage(); + +extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); + +PCA9685::PCA9685(int bus, uint8_t address) : + I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), + _mode(IOX_MODE_OFF), + _running(false), + _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _should_run(false), + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), + _actuator_controls_sub(-1), + _actuator_controls(), + _mode_on_initialized(false) +{ + memset(&_work, 0, sizeof(_work)); + memset(_msg, 0, sizeof(_msg)); + memset(_current_values, 0, sizeof(_current_values)); +} + +PCA9685::~PCA9685() +{ +} + +int +PCA9685::init() +{ + int ret; + ret = I2C::init(); + if (ret != OK) { + return ret; + } + + ret = reset(); + if (ret != OK) { + return ret; + } + + ret = setPWMFreq(PCA9685_PWMFREQ); + + return ret; +} + +int +PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + switch (cmd) { + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + warnx("shutting down"); + break; + case IOX_MODE_ON: + warnx("starting"); + break; + case IOX_MODE_TEST_OUT: + warnx("test starting"); + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + } + + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1); + } + + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +int +PCA9685::info() +{ + int ret = OK; + + if (is_running()) { + warnx("Driver is running, mode: %u", _mode); + } else { + warnx("Driver started but not running"); + } + + return ret; +} + +void +PCA9685::i2cpwm_trampoline(void *arg) +{ + PCA9685 *i2cpwm = reinterpret_cast(arg); + + i2cpwm->i2cpwm(); +} + +/** + * Main loop function + */ +void +PCA9685::i2cpwm() +{ + if (_mode == IOX_MODE_TEST_OUT) { + setPin(0, PCA9685_CENTER); + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _should_run = false; + } else { + if (!_mode_on_initialized) { + /* Subscribe to actuator control 2 (payload group for gimbal) */ + _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); + /* set the uorb update interval lower than the driver pwm interval */ + orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5); + + _mode_on_initialized = true; + } + + /* Read the servo setpoints from the actuator control topics (gimbal) */ + bool updated; + orb_check(_actuator_controls_sub, &updated); + if (updated) { + orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + uint16_t new_value = PCA9685_CENTER + + (_actuator_controls.control[i] * PCA9685_SCALE); + debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && + isfinite(new_value) && + new_value >= 0 && + new_value <= PCA9685_SERVOMAX) { + /* This value was updated, send the command to adjust the PWM value */ + setPin(i, new_value); + _current_values[i] = new_value; + } + } + } + _should_run = true; + } + + // check if any activity remains, else stop + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); +} + +int +PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) +{ + int ret; + /* convert to correct message */ + _msg[0] = LED0_ON_L + 4 * num; + _msg[1] = on; + _msg[2] = on >> 8; + _msg[3] = off; + _msg[4] = off >> 8; + + /* try i2c transfer */ + ret = transfer(_msg, 5, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + + return ret; +} + +int +PCA9685::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + if (val > 4095) { + val = 4095; + } + if (invert) { + if (val == 0) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, 4095-val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, val); + } + } + + return ERROR; +} + +int +PCA9685::setPWMFreq(float freq) +{ + int ret = OK; + freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue + https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() + uint8_t oldmode; + ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + + usleep(5000); //5ms delay (from arduino driver) + + ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { + return ret; + } + + return ret; +} + +/* Wrapper to read a byte from addr */ +int +PCA9685::read8(uint8_t addr, uint8_t &value) +{ + int ret = OK; + + /* send addr */ + ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { + goto fail_read; + } + + /* get value */ + ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { + goto fail_read; + } + + return ret; + +fail_read: + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + + return ret; +} + +int PCA9685::reset(void) { + warnx("resetting"); + return write8(PCA9685_MODE1, 0x0); +} + +/* Wrapper to wite a byte to addr */ +int +PCA9685::write8(uint8_t addr, uint8_t value) { + int ret = OK; + _msg[0] = addr; + _msg[1] = value; + /* send addr and value */ + ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + return ret; +} + +void +pca9685_usage() +{ + warnx("missing command: try 'start', 'test', 'stop', 'info'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca9685_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2caddr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2caddr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca9685_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca9685_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca9685 != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr); + + if (g_pca9685 != nullptr && OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; + } + + if (g_pca9685 == nullptr) { + errx(1, "init failed"); + } + } + + if (g_pca9685 == nullptr) { + g_pca9685 = new PCA9685(i2cdevice, i2caddr); + + if (g_pca9685 == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; + errx(1, "init failed"); + } + } + fd = open(PCA9685_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); + close(fd); + + + exit(0); + } + + // need the driver past this point + if (g_pca9685 == nullptr) { + warnx("not started, run pca9685 start"); + exit(1); + } + + if (!strcmp(verb, "info")) { + g_pca9685->info(); + exit(0); + } + + if (!strcmp(verb, "reset")) { + g_pca9685->reset(); + exit(0); + } + + + if (!strcmp(verb, "test")) { + fd = open(PCA9685_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(PCA9685_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca9685->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca9685->is_running()) { + delete g_pca9685; + g_pca9685= nullptr; + warnx("stopped, exiting"); + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + pca9685_usage(); + exit(0); +} -- cgit v1.2.3 From 5bc81c8561bbbad3f63d630a7adb40405623a626 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 09:28:46 +0200 Subject: pca9685: correct input scaling --- src/drivers/pca9685/pca9685.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index c97d1e9ab..6f69ce8a1 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -98,13 +98,15 @@ #define PCA9685_PWMFREQ 60.0f #define PCA9685_NCHANS 16 // total amount of pwm outputs -#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) -#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f +#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2) -#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE) -#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees -#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) +#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2) +#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees + PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG + PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG + */ +#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -300,7 +302,7 @@ void PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { - setPin(0, PCA9685_CENTER); + setPin(0, PCA9685_PWMCENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; @@ -320,14 +322,16 @@ PCA9685::i2cpwm() if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - uint16_t new_value = PCA9685_CENTER + - (_actuator_controls.control[i] * PCA9685_SCALE); + /* Scale the controls to PWM, first multiply by pi to get rad, + * the control[i] values are on the range -1 ... 1 */ + uint16_t new_value = PCA9685_PWMCENTER + + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && - new_value >= 0 && - new_value <= PCA9685_SERVOMAX) { + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; -- cgit v1.2.3 From 3e1de713af500bfa97e5e27f588d76b2ddaafc01 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 09:39:43 +0200 Subject: navigator: correct mode for land and termination --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..042c46afd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -353,6 +353,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,8 +370,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: _navigation_mode = &_offboard; break; -- cgit v1.2.3