mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 16:08:28 -04:00
f6ff1742d5
Barometer climb rate must be -150cm/s ~ +150cm/s This threshold is generous because we already use the inertial navigation climb rate so this is just to catch cases where inertial nav is very incorrect in it's climbrate estimates
298 lines
8.3 KiB
Plaintext
298 lines
8.3 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
// These are function definitions so the Menu can be constructed before the functions
|
|
// are defined below. Order matters to the compiler.
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
|
|
#endif
|
|
static int8_t test_compass(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
static int8_t test_shell(uint8_t argc, const Menu::arg *argv);
|
|
#endif
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
|
#endif
|
|
|
|
// Creates a constant array of structs representing menu options
|
|
// and stores them in Flash memory, not RAM.
|
|
// User enters the string in the console to call the functions on the right.
|
|
// See class Menu in AP_Coommon for implementation details
|
|
const struct Menu::command test_menu_commands[] PROGMEM = {
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
{"baro", test_baro},
|
|
#endif
|
|
{"compass", test_compass},
|
|
{"ins", test_ins},
|
|
{"optflow", test_optflow},
|
|
{"relay", test_relay},
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
{"shell", test_shell},
|
|
#endif
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
{"rangefinder", test_sonar},
|
|
#endif
|
|
};
|
|
|
|
// A Macro to create the Menu
|
|
MENU(test_menu, "test", test_menu_commands);
|
|
|
|
static int8_t
|
|
test_mode(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
test_menu.run();
|
|
return 0;
|
|
}
|
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
static int8_t
|
|
test_baro(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
int32_t alt;
|
|
print_hit_enter();
|
|
init_barometer(true);
|
|
|
|
while(1) {
|
|
delay(100);
|
|
read_barometer();
|
|
|
|
if (!barometer.healthy()) {
|
|
cliSerial->println_P(PSTR("not healthy"));
|
|
} else {
|
|
cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"),
|
|
baro_alt / 100.0,
|
|
barometer.get_pressure(),
|
|
barometer.get_temperature());
|
|
}
|
|
if(cliSerial->available() > 0) {
|
|
return (0);
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
static int8_t
|
|
test_compass(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
uint8_t delta_ms_fast_loop;
|
|
uint8_t medium_loopCounter = 0;
|
|
|
|
if (!g.compass_enabled) {
|
|
cliSerial->printf_P(PSTR("Compass: "));
|
|
print_enabled(false);
|
|
return (0);
|
|
}
|
|
|
|
if (!compass.init()) {
|
|
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
|
return 0;
|
|
}
|
|
|
|
ahrs.init();
|
|
ahrs.set_fly_forward(true);
|
|
ahrs.set_compass(&compass);
|
|
report_compass();
|
|
|
|
// we need the AHRS initialised for this test
|
|
ins.init(AP_InertialSensor::COLD_START,
|
|
ins_sample_rate);
|
|
ahrs.reset();
|
|
int16_t counter = 0;
|
|
float heading = 0;
|
|
|
|
print_hit_enter();
|
|
|
|
while(1) {
|
|
delay(20);
|
|
if (millis() - fast_loopTimer > 19) {
|
|
delta_ms_fast_loop = millis() - fast_loopTimer;
|
|
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
|
fast_loopTimer = millis();
|
|
|
|
// INS
|
|
// ---
|
|
ahrs.update();
|
|
|
|
medium_loopCounter++;
|
|
if(medium_loopCounter == 5) {
|
|
if (compass.read()) {
|
|
// Calculate heading
|
|
const Matrix3f &m = ahrs.get_dcm_matrix();
|
|
heading = compass.calculate_heading(m);
|
|
compass.learn_offsets();
|
|
}
|
|
medium_loopCounter = 0;
|
|
}
|
|
|
|
counter++;
|
|
if (counter>20) {
|
|
if (compass.healthy()) {
|
|
const Vector3f &mag_ofs = compass.get_offsets();
|
|
const Vector3f &mag = compass.get_field();
|
|
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
|
|
(wrap_360_cd(ToDeg(heading) * 100)) /100,
|
|
mag.x,
|
|
mag.y,
|
|
mag.z,
|
|
mag_ofs.x,
|
|
mag_ofs.y,
|
|
mag_ofs.z);
|
|
} else {
|
|
cliSerial->println_P(PSTR("compass not healthy"));
|
|
}
|
|
counter=0;
|
|
}
|
|
}
|
|
if (cliSerial->available() > 0) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// save offsets. This allows you to get sane offset values using
|
|
// the CLI before you go flying.
|
|
cliSerial->println_P(PSTR("saving offsets"));
|
|
compass.save_offsets();
|
|
return (0);
|
|
}
|
|
|
|
static int8_t
|
|
test_ins(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Vector3f gyro, accel;
|
|
print_hit_enter();
|
|
cliSerial->printf_P(PSTR("INS\n"));
|
|
delay(1000);
|
|
|
|
ahrs.init();
|
|
ins.init(AP_InertialSensor::COLD_START,
|
|
ins_sample_rate);
|
|
cliSerial->printf_P(PSTR("...done\n"));
|
|
|
|
delay(50);
|
|
|
|
while(1) {
|
|
ins.update();
|
|
gyro = ins.get_gyro();
|
|
accel = ins.get_accel();
|
|
|
|
float test = accel.length() / GRAVITY_MSS;
|
|
|
|
cliSerial->printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n"),
|
|
accel.x, accel.y, accel.z,
|
|
gyro.x, gyro.y, gyro.z,
|
|
test);
|
|
|
|
delay(40);
|
|
if(cliSerial->available() > 0) {
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_optflow(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
#if OPTFLOW == ENABLED
|
|
if(optflow.enabled()) {
|
|
cliSerial->printf_P(PSTR("dev id: %d\t"),(int)optflow.device_id());
|
|
print_hit_enter();
|
|
|
|
while(1) {
|
|
delay(200);
|
|
optflow.update();
|
|
const Vector2i& raw = optflow.raw();
|
|
cliSerial->printf_P(PSTR("dx:%d\t dy:%d\t squal:%d\n"),
|
|
(int)raw.x,
|
|
(int)raw.y,
|
|
(int)optflow.quality());
|
|
|
|
if(cliSerial->available() > 0) {
|
|
return (0);
|
|
}
|
|
}
|
|
} else {
|
|
cliSerial->printf_P(PSTR("OptFlow: "));
|
|
print_enabled(false);
|
|
}
|
|
return (0);
|
|
#else
|
|
return (0);
|
|
#endif // OPTFLOW == ENABLED
|
|
}
|
|
|
|
static int8_t test_relay(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1) {
|
|
cliSerial->printf_P(PSTR("Relay on\n"));
|
|
relay.on(0);
|
|
delay(3000);
|
|
if(cliSerial->available() > 0) {
|
|
return (0);
|
|
}
|
|
|
|
cliSerial->printf_P(PSTR("Relay off\n"));
|
|
relay.off(0);
|
|
delay(3000);
|
|
if(cliSerial->available() > 0) {
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
/*
|
|
* run a debug shell
|
|
*/
|
|
static int8_t
|
|
test_shell(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
hal.util->run_debug_shell(cliSerial);
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
/*
|
|
* test the rangefinders
|
|
*/
|
|
static int8_t
|
|
test_sonar(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
#if CONFIG_SONAR == ENABLED
|
|
sonar.init();
|
|
|
|
cliSerial->printf_P(PSTR("RangeFinder: %d devices detected\n"), sonar.num_sensors());
|
|
|
|
print_hit_enter();
|
|
while(1) {
|
|
delay(100);
|
|
sonar.update();
|
|
|
|
cliSerial->printf_P(PSTR("Primary: health %d distance_cm %d \n"), (int)sonar.healthy(), sonar.distance_cm());
|
|
cliSerial->printf_P(PSTR("All: device_0 type %d health %d distance_cm %d, device_1 type %d health %d distance_cm %d\n"),
|
|
(int)sonar._type[0], (int)sonar.healthy(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.healthy(1), sonar.distance_cm(1));
|
|
|
|
if(cliSerial->available() > 0) {
|
|
return (0);
|
|
}
|
|
}
|
|
#endif
|
|
return (0);
|
|
}
|
|
#endif
|
|
|
|
static void print_hit_enter()
|
|
{
|
|
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
|
|
}
|
|
|
|
#endif // CLI_ENABLED
|