ardupilot/libraries/AP_InertialSensor/examples/PX4/PX4.pde

200 lines
4.7 KiB
Plaintext
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// Simple test for the AP_InertialSensor driver.
//
#include <stdarg.h>
#include <AP_Common.h>
#include <AP_Progmem.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_Param.h>
#include <AP_ADC.h>
#include <AP_InertialSensor.h>
2013-08-29 02:05:30 -03:00
#include <AP_Notify.h>
2013-12-16 20:50:51 -04:00
#include <AP_GPS.h>
#include <AP_Baro.h>
#include <Filter.h>
2013-12-16 20:50:51 -04:00
#include <DataFlash.h>
#include <GCS_MAVLink.h>
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
AP_InertialSensor_PX4 ins;
void setup(void)
{
hal.console->println("AP_InertialSensor startup...");
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_100HZ,
NULL);
// display initial values
display_offsets_and_scaling();
hal.console->println("Complete. Reading:");
}
void loop(void)
{
int16_t user_input;
hal.console->println();
hal.console->println_P(PSTR(
"Menu:\r\n"
" c) calibrate accelerometers\r\n"
" d) display offsets and scaling\r\n"
" l) level (capture offsets from level)\r\n"
" t) test\r\n"
" r) reboot"));
// wait for user input
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
// read in user input
while( hal.console->available() ) {
user_input = hal.console->read();
if( user_input == 'c' || user_input == 'C' ) {
run_calibration();
display_offsets_and_scaling();
}
if( user_input == 'd' || user_input == 'D' ) {
display_offsets_and_scaling();
}
if( user_input == 'l' || user_input == 'L' ) {
run_level();
display_offsets_and_scaling();
}
if( user_input == 't' || user_input == 'T' ) {
run_test();
}
if( user_input == 'r' || user_input == 'R' ) {
2013-09-07 08:03:59 -03:00
hal.scheduler->reboot(false);
}
}
}
void run_calibration()
{
// clear off any other characters (like line feeds,etc)
while( hal.console->available() ) {
hal.console->read();
}
AP_InertialSensor_UserInteractStream interact(hal.console);
ins.calibrate_accel(NULL, &interact);
}
void display_offsets_and_scaling()
{
Vector3f accel_offsets = ins.get_accel_offsets();
Vector3f accel_scale = ins.get_accel_scale();
Vector3f gyro_offsets = ins.get_gyro_offsets();
// display results
hal.console->printf_P(
PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
hal.console->printf_P(
PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
accel_scale.x,
accel_scale.y,
accel_scale.z);
hal.console->printf_P(
PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
gyro_offsets.x,
gyro_offsets.y,
gyro_offsets.z);
}
void run_level()
{
// clear off any input in the buffer
while( hal.console->available() ) {
hal.console->read();
}
// display message to user
hal.console->print("Place APM on a level surface and press any key..\n");
// wait for user input
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
while( hal.console->available() ) {
hal.console->read();
}
// run accel level
ins.init_accel();
// display results
display_offsets_and_scaling();
}
void run_test()
{
Vector3f accel;
Vector3f gyro;
float length;
uint8_t counter = 0;
// flush any user input
while( hal.console->available() ) {
hal.console->read();
}
// clear out any existing samples from ins
ins.update();
// loop as long as user does not press a key
while( !hal.console->available() ) {
// wait until we have a sample
ins.wait_for_sample();
// read samples from ins
ins.update();
accel = ins.get_accel();
gyro = ins.get_gyro();
length = accel.length();
if (counter++ % 50 == 0) {
// display results
hal.console->printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f \t dt:%u\n"),
accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z,
(unsigned)(1.0e6*ins.get_delta_time()));
}
}
// clear user input
while( hal.console->available() ) {
hal.console->read();
}
}
#else
void setup() {}
void loop() {}
#endif // CONFIG_HAL_BOARD
AP_HAL_MAIN();