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

pwm_input: astyle

This commit is contained in:
Ban Siesta 2015-05-24 09:42:34 +01:00
parent bd48ef0386
commit 3efaeabd5b
1 changed files with 117 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

205
src/drivers/pwm_input/pwm_input.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file pwm_input.cpp
* @file pwm_input.cpp
*
* PWM input driver based on earlier driver from Evan Slatyer,
* which in turn was based on drv_hrt.c
@ -192,32 +192,32 @@
* Specific registers and bits used by HRT sub-functions
*/
#if PWMIN_TIMER_CHANNEL == 1
#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#elif PWMIN_TIMER_CHANNEL == 2
#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#else
#error PWMIN_TIMER_CHANNEL must be either 1 and 2.
#error PWMIN_TIMER_CHANNEL must be either 1 and 2.
#endif
#define TIMEOUT 300000 /* reset after no responce over this time in microseconds [0.3 secs] */
@ -235,18 +235,18 @@ public:
void _publish(uint16_t status, uint32_t period, uint32_t pulse_width);
void _print_info(void);
void hard_reset();
void hard_reset();
private:
uint32_t error_count;
uint32_t pulses_captured;
uint32_t last_period;
uint32_t last_width;
uint64_t last_poll_time;
uint64_t last_poll_time;
RingBuffer *reports;
bool timer_started;
range_finder_report data;
range_finder_report data;
orb_advert_t range_finder_pub;
hrt_call hard_reset_call; // HRT callout for note completion
@ -254,9 +254,9 @@ private:
void timer_init(void);
void turn_on();
void turn_off();
void freeze_test();
void turn_on();
void turn_off();
void freeze_test();
};
@ -276,15 +276,16 @@ PWMIN::PWMIN() :
last_width(0),
reports(nullptr),
timer_started(false),
range_finder_pub(-1)
range_finder_pub(-1)
{
memset(&data, 0, sizeof(data));
}
PWMIN::~PWMIN()
{
if (reports != nullptr)
if (reports != nullptr) {
delete reports;
}
}
/*
@ -300,21 +301,22 @@ PWMIN::init()
// activate the timer when requested to when the device is opened
CDev::init();
data.type = RANGE_FINDER_TYPE_LASER;
data.minimum_distance = 0.20f;
data.maximum_distance = 7.0f;
data.type = RANGE_FINDER_TYPE_LASER;
data.minimum_distance = 0.20f;
data.maximum_distance = 7.0f;
range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data);
fprintf(stderr, "[pwm_input] advertising %d\n"
,range_finder_pub);
range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data);
fprintf(stderr, "[pwm_input] advertising %d\n"
, range_finder_pub);
reports = new RingBuffer(2, sizeof(struct pwm_input_s));
if (reports == nullptr) {
return -ENOMEM;
}
// Schedule freeze check to invoke periodically
hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast<hrt_callout>(&PWMIN::freeze_test), this);
// Schedule freeze check to invoke periodically
hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast<hrt_callout>(&PWMIN::freeze_test), this);
return OK;
}
@ -322,7 +324,7 @@ PWMIN::init()
/*
* Initialise the timer we are going to use.
*/
void PWMIN::timer_init(void)
void PWMIN::timer_init(void)
{
// run with interrupts disabled in case the timer is already
// setup. We don't want it firing while we are doing the setup
@ -333,7 +335,7 @@ void PWMIN::timer_init(void)
irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr);
/* Clear no bits, set timer enable bit.*/
modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT);
modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT);
/* disable and configure the timer */
rCR1 = 0;
@ -351,10 +353,10 @@ void PWMIN::timer_init(void)
// for simplicity scale by the clock in MHz. This gives us
// readings in microseconds which is typically what is needed
// for a PWM input driver
uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL;
uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL;
/*
* define the clock speed. We want the highest possible clock
/*
* define the clock speed. We want the highest possible clock
* speed that avoids overflows.
*/
rPSC = prescaler - 1;
@ -380,13 +382,14 @@ void PWMIN::timer_init(void)
void
PWMIN::freeze_test()
{
/* timeout is true if least read was away back */
bool timeout = false;
timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false;
if (timeout) {
fprintf(stderr, "[pwm_input] Lidar is down, reseting\n");
hard_reset();
}
/* timeout is true if least read was away back */
bool timeout = false;
timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false;
if (timeout) {
fprintf(stderr, "[pwm_input] Lidar is down, reseting\n");
hard_reset();
}
}
void
@ -398,8 +401,8 @@ PWMIN::turn_off() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); }
void
PWMIN::hard_reset()
{
turn_off();
hrt_call_after(&hard_reset_call, 9000, reinterpret_cast<hrt_callout>(&PWMIN::turn_on), this);
turn_off();
hrt_call_after(&hard_reset_call, 9000, reinterpret_cast<hrt_callout>(&PWMIN::turn_on), this);
}
/*
@ -412,10 +415,13 @@ PWMIN::open(struct file *filp)
if (g_dev == nullptr) {
return -EIO;
}
int ret = CDev::open(filp);
int ret = CDev::open(filp);
if (ret == OK && !timer_started) {
g_dev->timer_init();
}
return ret;
}
@ -429,14 +435,17 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 500))
if ((arg < 1) || (arg > 500)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
@ -471,8 +480,9 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
while (count--) {
if (reports->get(buf)) {
@ -496,7 +506,7 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width)
return;
}
last_poll_time = hrt_absolute_time();
last_poll_time = hrt_absolute_time();
struct pwm_input_s pwmin_report;
pwmin_report.timestamp = last_poll_time;
@ -504,18 +514,20 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width)
pwmin_report.period = period;
pwmin_report.pulse_width = pulse_width;
data.distance = pulse_width * 1e-3f;
data.timestamp = pwmin_report.timestamp;
data.error_count = error_count;
data.distance = pulse_width * 1e-3f;
data.timestamp = pwmin_report.timestamp;
data.error_count = error_count;
if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) {
data.valid = false;
} else {
data.valid = true;
}
if (range_finder_pub > 0) {
orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data);
}
if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) {
data.valid = false;
} else {
data.valid = true;
}
if (range_finder_pub > 0) {
orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data);
}
reports->force(&pwmin_report);
@ -531,6 +543,7 @@ void PWMIN::_print_info(void)
{
if (!timer_started) {
printf("timer not started - try the 'test' command\n");
} else {
printf("count=%u period=%u width=%u\n",
(unsigned)pulses_captured,
@ -568,20 +581,27 @@ static void pwmin_start(bool full_start)
printf("driver already started\n");
exit(1);
}
g_dev = new PWMIN();
if (g_dev == nullptr) {
errx(1, "driver allocation failed");
}
if (g_dev->init() != OK) {
errx(1, "driver init failed");
}
if (full_start) {
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open device");
}
close(fd);
}
if (full_start) {
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open device");
}
close(fd);
}
exit(0);
}
@ -591,15 +611,18 @@ static void pwmin_start(bool full_start)
static void pwmin_test(void)
{
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open device");
}
uint64_t start_time = hrt_absolute_time();
printf("Showing samples for 5 seconds\n");
while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) {
while (hrt_absolute_time() < start_time + 5U * 1000UL * 1000UL) {
struct pwm_input_s buf;
if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) {
printf("period=%u width=%u error_count=%u\n",
(unsigned)buf.period,
@ -607,6 +630,7 @@ static void pwmin_test(void)
(unsigned)buf.error_count);
}
}
close(fd);
exit(0);
}
@ -616,14 +640,17 @@ static void pwmin_test(void)
*/
static void pwmin_reset(void)
{
g_dev->hard_reset();
g_dev->hard_reset();
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open device");
}
if (ioctl(fd, SENSORIOCRESET, 0) != OK) {
errx(1, "reset failed");
}
close(fd);
exit(0);
}
@ -637,6 +664,7 @@ static void pwmin_info(void)
printf("driver not started\n");
exit(1);
}
g_dev->_print_info();
exit(0);
}
@ -645,22 +673,23 @@ static void pwmin_info(void)
/*
driver entry point
*/
int pwm_input_main(int argc, char * argv[])
int pwm_input_main(int argc, char *argv[])
{
const char *verb = argv[1];
/*
* init driver and start reading
*/
bool full_start = false;
if (!strcmp(argv[2], "full")) {
full_start = true;
}
/*
* init driver and start reading
*/
bool full_start = false;
if (!strcmp(argv[2], "full")) {
full_start = true;
}
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
pwmin_start(full_start);
pwmin_start(full_start);
}
/*