RangeFinder: add PulsedLight example sketch

This commit is contained in:
Randy Mackay 2013-12-02 20:05:57 +09:00
parent e88251f7e1
commit 3970c8f038
3 changed files with 68 additions and 0 deletions

View File

@ -0,0 +1,2 @@
BOARD = mega
include ../../../../mk/apm.mk

View File

@ -0,0 +1,66 @@
/*
* AP_RangeFinder_test
* Code by DIYDrones.com
*/
// includes
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include <AP_HAL_PX4.h>
#include <AP_HAL_Empty.h>
#include <AP_Math.h>
#include <AP_RangeFinder.h>
#include <Filter.h>
#include <AP_Buffer.h>
////////////////////////////////////////////////////////////////////////////////
// hal.console-> ports
////////////////////////////////////////////////////////////////////////////////
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
// declare global instances
ModeFilterInt16_Size5 mode_filter(2);
AP_RangeFinder_PulsedLightLRF lrf(&mode_filter);
void setup()
{
// print welcome message
hal.console->println("PulsedLight LRF Test v1.1");
// ensure i2c is slow
hal.i2c->setHighSpeed(false);
// initialise sensor
lrf.init();
// kick off one reading
lrf.take_reading();
// check health
if (!lrf.healthy) {
hal.console->println("Initialisation failed");
}
}
void loop()
{
int16_t distance;
// ask sensor take a reading
//lrf.take_reading();
hal.scheduler->delay(50);
// get distance
distance = lrf.read();
// display output
//hal.console->printf_P(PSTR("dist: %d \thealth:%d \tb0:%d b1:%d\n"),(int)distance,(int)lrf.healthy,(int)lrf._buff[0], (int)lrf._buff[1]);
//hal.console->printf_P(PSTR("dist: %d\n"),(int)distance);
hal.console->printf_P(PSTR("dist: %d %d\n"),(int)distance,(int)lrf.healthy);
}
AP_HAL_MAIN();