dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

adafruit i2c pwm: listen to uorb for setpoints

This commit is contained in:
Thomas Gubler 2014-08-20 15:04:24 +02:00
parent 66991f9bb1
commit dd85c0407c
1 changed files with 49 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/drivers/adafruiti2cpwm/adafruiti2cpwm.cpp
View File

@ -59,6 +59,7 @@
#include <unistd.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
@ -67,6 +68,9 @@
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <board_config.h>
#include <drivers/drv_io_expander.h>
@ -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;
}