3 changed files with 68 additions and 0 deletions
@ -0,0 +1,2 @@ |
|||||||
|
BOARD = mega
|
||||||
|
include ../../../../mk/apm.mk |
@ -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(); |
Loading…
Reference in new issue