From da0e3169f2796d3223b4f7857da067e9aac2fea4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 18:32:51 +0200 Subject: fw att control: change control surface deflection scaling --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 ++++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 46db788a6..926a8db2a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9894a34d7..94bd26f03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } -- cgit v1.2.3 From c3522f85928c16d55e54dedc73eb4ed1420d527f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 10:58:28 +0200 Subject: Publish telemetry status on telemetry update and on heartbeat update events to avoid inducing heartbeat update latencies resulting in spurious telemetry link dropped detections. Makes overall state handling simpler --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 30 +++++++++--------------------- src/modules/mavlink/mavlink_receiver.h | 1 - 3 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2faf8ab76..c27716f74 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -217,6 +217,8 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0fae0a2f..e9a9ff133 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -430,9 +429,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -474,25 +470,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); - - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; - - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 014193100..3999b8b01 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -151,7 +151,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - bool _radio_status_available; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; -- cgit v1.2.3 From 01f1c90c2673754c10d031796b0dbba5e7a6fd55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:48:38 +0200 Subject: Make some space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a7c10f52f..97eddfdd2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,7 +25,6 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf @@ -152,5 +150,4 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) + $(call _B, serdis, , 2048, serdis_main ) -- 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(-) 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 024c6d572706b3ae7755b5c022e198d7db7c5d34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:49:12 +0200 Subject: Airspeed fix attempt --- src/drivers/airspeed/airspeed.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 41942aacd..29110373f 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -364,6 +364,7 @@ Airspeed::update_status() _sensor_ok, SUBSYSTEM_TYPE_DIFFPRESSURE }; + warnx("airspeed ok state changed"); if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); @@ -381,7 +382,10 @@ Airspeed::cycle_trampoline(void *arg) Airspeed *dev = (Airspeed *)arg; dev->cycle(); - dev->update_status(); + // XXX we do not know if this is + // really helping - do not update the + // subsys state right now + //dev->update_status(); } void -- 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(+) 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 9e82f14ad805dab3be7fcba701430db54c4b339f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 18:53:37 +0200 Subject: Mag scale check: only test if the scale roughly makes sense, do not judge the environment --- src/systemcmds/tests/test_sensors.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index a4f17eebd..e005bf9c1 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -331,7 +331,7 @@ mag(int argc, char *argv[]) float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); - if (len < 1.0f || len > 3.0f) { + if (len < 0.25f || len > 3.0f) { warnx("MAG scale error!"); return ERROR; } -- cgit v1.2.3 From 8d7a12218ca4305e3ca36320584113773e550091 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 20:06:30 +0200 Subject: Time out after a reasonable interval (10 seconds, as e.g. OBC rules prescribe). Experiments show the SiK radios to time out ~4-7 seconds if they loose sync --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c2c03070..74deda8cc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -129,7 +129,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 +#define DL_TIMEOUT (10 * 1000 * 1000) #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 -- cgit v1.2.3 From 7eb521b6d4643fbf36d20e923faadca4e8f61dc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 20:07:55 +0200 Subject: Do not perform retries in airspeed driver - this is too much load for the HRT queue --- src/drivers/airspeed/airspeed.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 29110373f..293690d27 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -165,7 +165,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 2; + _retries = 0; return ret; } @@ -364,7 +364,6 @@ Airspeed::update_status() _sensor_ok, SUBSYSTEM_TYPE_DIFFPRESSURE }; - warnx("airspeed ok state changed"); if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); -- 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(-) 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 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(-) 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(-) 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 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