ardupilot/ArduPlane/test.cpp

532 lines
15 KiB
C++
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2015-05-13 03:09:36 -03:00
#include "Plane.h"
2015-05-13 03:09:36 -03:00
#if CLI_ENABLED == ENABLED
2014-03-03 09:50:45 -04:00
// 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_Common for implementation details
static const struct Menu::command test_menu_commands[] = {
2015-05-13 03:09:36 -03:00
{"pwm", MENU_FUNC(test_radio_pwm)},
{"radio", MENU_FUNC(test_radio)},
{"passthru", MENU_FUNC(test_passthru)},
{"failsafe", MENU_FUNC(test_failsafe)},
{"relay", MENU_FUNC(test_relay)},
{"waypoints", MENU_FUNC(test_wp)},
{"xbee", MENU_FUNC(test_xbee)},
{"modeswitch", MENU_FUNC(test_modeswitch)},
2012-08-21 23:19:51 -03:00
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
2015-05-13 03:09:36 -03:00
{"gps", MENU_FUNC(test_gps)},
{"ins", MENU_FUNC(test_ins)},
{"airspeed", MENU_FUNC(test_airspeed)},
{"airpressure", MENU_FUNC(test_pressure)},
{"compass", MENU_FUNC(test_mag)},
{"logging", MENU_FUNC(test_logging)},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
2015-05-13 03:09:36 -03:00
{"shell", MENU_FUNC(test_shell)},
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
2015-05-13 03:09:36 -03:00
int8_t Plane::test_mode(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Test Mode\n\n");
2012-08-21 23:19:51 -03:00
test_menu.run();
return 0;
}
2015-05-13 03:09:36 -03:00
void Plane::print_hit_enter()
{
cliSerial->printf("Hit Enter to exit.\n\n");
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
while(1) {
hal.scheduler->delay(20);
2012-08-21 23:19:51 -03:00
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
(int)channel_roll->get_radio_in(),
(int)channel_pitch->get_radio_in(),
(int)channel_throttle->get_radio_in(),
(int)channel_rudder->get_radio_in(),
(int)g.rc_5.get_radio_in(),
(int)g.rc_6.get_radio_in(),
(int)g.rc_7.get_radio_in(),
(int)g.rc_8.get_radio_in());
2012-08-21 23:19:51 -03:00
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
while(1) {
hal.scheduler->delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
2014-03-25 00:41:14 -03:00
if (hal.rcin->new_input()) {
2015-10-25 16:22:31 -03:00
cliSerial->print("CH:");
2012-08-21 23:19:51 -03:00
for(int16_t i = 0; i < 8; i++) {
2012-12-04 18:22:21 -04:00
cliSerial->print(hal.rcin->read(i)); // Print channel values
2012-09-18 00:48:08 -03:00
print_comma();
servo_write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->println();
}
if (cliSerial->available() > 0) {
return (0);
}
}
return 0;
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
// read the radio to set trims
// ---------------------------
trim_radio();
while(1) {
hal.scheduler->delay(20);
2012-08-21 23:19:51 -03:00
read_radio();
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
channel_throttle->calc_pwm();
channel_rudder->calc_pwm();
2012-08-21 23:19:51 -03:00
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
(int)channel_roll->get_control_in(),
(int)channel_pitch->get_control_in(),
(int)channel_throttle->get_control_in(),
(int)channel_rudder->get_control_in(),
(int)g.rc_5.get_control_in(),
(int)g.rc_6.get_control_in(),
(int)g.rc_7.get_control_in(),
(int)g.rc_8.get_control_in() );
2012-08-21 23:19:51 -03:00
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test = 0;
2012-08-21 23:19:51 -03:00
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
2012-08-21 23:19:51 -03:00
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n");
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
while(channel_throttle->get_control_in() > 0) {
hal.scheduler->delay(20);
2012-08-21 23:19:51 -03:00
read_radio();
}
while(1) {
hal.scheduler->delay(20);
2012-08-21 23:19:51 -03:00
read_radio();
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
if(channel_throttle->get_control_in() > 0) {
cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->get_control_in());
2012-08-21 23:19:51 -03:00
fail_test++;
}
if(oldSwitchPosition != readSwitch()) {
cliSerial->printf("CONTROL MODE CHANGED: ");
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
2012-08-21 23:19:51 -03:00
fail_test++;
}
if(rc_failsafe_active()) {
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->get_radio_in());
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
2012-08-21 23:19:51 -03:00
fail_test++;
}
if(fail_test > 0) {
return (0);
}
if(cliSerial->available() > 0) {
cliSerial->printf("LOS caused no change in APM.\n");
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
while(1) {
cliSerial->printf("Relay on\n");
relay.on(0);
hal.scheduler->delay(3000);
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
cliSerial->printf("Relay off\n");
relay.off(0);
hal.scheduler->delay(3000);
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
// save the alitude above home option
if (g.RTL_altitude_cm < 0) {
cliSerial->printf("Hold current altitude\n");
2012-08-21 23:19:51 -03:00
}else{
cliSerial->printf("Hold altitude of %dm\n", (int)g.RTL_altitude_cm/100);
2012-08-21 23:19:51 -03:00
}
cliSerial->printf("%d waypoints\n", (int)mission.num_commands());
cliSerial->printf("Hit radius: %d\n", (int)g.waypoint_radius);
cliSerial->printf("Loiter radius: %d\n\n", (int)g.loiter_radius);
2014-03-03 09:50:45 -04:00
for(uint8_t i = 0; i <= mission.num_commands(); i++) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(i,temp_cmd)) {
test_wp_print(temp_cmd);
}
2012-08-21 23:19:51 -03:00
}
2012-08-21 23:19:51 -03:00
return (0);
}
2015-05-13 03:09:36 -03:00
void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd)
{
cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n",
2014-03-03 09:50:45 -04:00
(int)cmd.index,
(int)cmd.id,
(int)cmd.content.location.options,
(int)cmd.p1,
(long)cmd.content.location.alt,
(long)cmd.content.location.lat,
(long)cmd.content.location.lng);
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
cliSerial->printf("Begin XBee X-CTU Range and RSSI Test:\n");
2012-08-21 23:19:51 -03:00
while(1) {
2012-12-04 18:22:21 -04:00
if (hal.uartC->available())
hal.uartC->write(hal.uartC->read());
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
cliSerial->printf("Control CH ");
2012-08-21 23:19:51 -03:00
cliSerial->println(FLIGHT_MODE_CHANNEL, BASE_DEC);
2012-08-21 23:19:51 -03:00
while(1) {
hal.scheduler->delay(20);
2012-12-04 18:22:21 -04:00
uint8_t switchPosition = readSwitch();
2012-08-21 23:19:51 -03:00
if (oldSwitchPosition != switchPosition) {
cliSerial->printf("Position %d\n", (int)switchPosition);
2012-08-21 23:19:51 -03:00
oldSwitchPosition = switchPosition;
}
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2011-12-28 00:53:14 -04:00
/*
2012-08-21 23:19:51 -03:00
* test the dataflash is working
2011-12-28 00:53:14 -04:00
*/
2015-05-13 03:09:36 -03:00
int8_t Plane::test_logging(uint8_t argc, const Menu::arg *argv)
2011-12-28 00:53:14 -04:00
{
2013-02-22 19:15:15 -04:00
DataFlash.ShowDeviceInfo(cliSerial);
2011-12-28 00:53:14 -04:00
return 0;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/*
* run a debug shell
*/
2015-05-13 03:09:36 -03:00
int8_t Plane::test_shell(uint8_t argc, const Menu::arg *argv)
{
hal.util->run_debug_shell(cliSerial);
return 0;
}
#endif
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
2015-05-13 03:09:36 -03:00
int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
2012-08-21 23:19:51 -03:00
2014-03-28 16:52:48 -03:00
uint32_t last_message_time_ms = 0;
2012-08-21 23:19:51 -03:00
while(1) {
hal.scheduler->delay(100);
2012-08-21 23:19:51 -03:00
2014-03-28 16:52:48 -03:00
gps.update();
2012-08-21 23:19:51 -03:00
2014-03-28 16:52:48 -03:00
if (gps.last_message_time_ms() != last_message_time_ms) {
last_message_time_ms = gps.last_message_time_ms();
const Location &loc = gps.location();
cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n",
2014-03-28 16:52:48 -03:00
(long)loc.lat,
(long)loc.lng,
(long)loc.alt/100,
(int)gps.num_sats());
} else {
cliSerial->printf(".");
2012-08-21 23:19:51 -03:00
}
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
{
//cliSerial->printf("Calibrating.");
2013-01-13 01:04:18 -04:00
ahrs.init();
ahrs.set_fly_forward(true);
2013-05-23 22:21:32 -03:00
ahrs.set_wind_estimation(true);
2013-01-13 01:04:18 -04:00
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
2012-08-21 23:19:51 -03:00
print_hit_enter();
hal.scheduler->delay(1000);
uint8_t counter = 0;
2012-08-21 23:19:51 -03:00
while(1) {
hal.scheduler->delay(20);
if (micros() - perf.fast_loopTimer_us > 19000UL) {
perf.fast_loopTimer_us = micros();
2012-08-21 23:19:51 -03:00
// INS
2012-08-21 23:19:51 -03:00
// ---
ahrs.update();
if(g.compass_enabled) {
counter++;
if(counter == 5) {
2012-08-21 23:19:51 -03:00
compass.read();
counter = 0;
2012-08-21 23:19:51 -03:00
}
}
// We are using the INS
2012-08-21 23:19:51 -03:00
// ---------------------
Vector3f gyros = ins.get_gyro();
Vector3f accels = ins.get_accel();
cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n",
(int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100,
(double)gyros.x, (double)gyros.y, (double)gyros.z,
(double)accels.x, (double)accels.y, (double)accels.z);
2012-08-21 23:19:51 -03:00
}
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
if (!g.compass_enabled) {
cliSerial->printf("Compass: ");
2012-08-21 23:19:51 -03:00
print_enabled(false);
return (0);
}
if (!compass.init()) {
cliSerial->println("Compass initialisation failed!");
return 0;
}
2013-01-13 01:04:18 -04:00
ahrs.init();
ahrs.set_fly_forward(true);
2013-05-23 22:21:32 -03:00
ahrs.set_wind_estimation(true);
ahrs.set_compass(&compass);
// we need the AHRS initialised for this test
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
uint16_t counter = 0;
float heading = 0;
print_hit_enter();
while(1) {
hal.scheduler->delay(20);
if (micros() - perf.fast_loopTimer_us > 19000UL) {
perf.fast_loopTimer_us = micros();
// INS
2012-08-21 23:19:51 -03:00
// ---
ahrs.update();
if(counter % 5 == 0) {
if (compass.read()) {
// Calculate heading
const Matrix3f &m = ahrs.get_rotation_body_to_ned();
heading = compass.calculate_heading(m);
compass.learn_offsets();
}
}
2012-08-21 23:19:51 -03:00
counter++;
if (counter>20) {
2013-12-09 02:47:38 -04:00
if (compass.healthy()) {
const Vector3f &mag_ofs = compass.get_offsets();
const Vector3f &mag = compass.get_field();
cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(double)((wrap_360_cd(ToDeg(heading) * 100)) /100),
(double)mag.x, (double)mag.y, (double)mag.z,
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
} else {
cliSerial->println("compass not healthy");
}
counter=0;
}
2012-08-21 23:19:51 -03:00
}
if (cliSerial->available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
2012-08-21 23:19:51 -03:00
// the CLI before you go flying.
cliSerial->println("saving offsets");
compass.save_offsets();
return (0);
}
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
2015-05-13 03:09:36 -03:00
int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv)
{
2012-08-21 23:19:51 -03:00
if (!airspeed.enabled()) {
cliSerial->printf("airspeed: ");
2012-08-21 23:19:51 -03:00
print_enabled(false);
return (0);
}else{
print_hit_enter();
2014-11-13 06:12:59 -04:00
zero_airspeed(false);
cliSerial->printf("airspeed: ");
2012-08-21 23:19:51 -03:00
print_enabled(true);
while(1) {
hal.scheduler->delay(20);
2012-08-21 23:19:51 -03:00
read_airspeed();
cliSerial->printf("%.1f m/s\n", (double)airspeed.get_airspeed());
2012-08-21 23:19:51 -03:00
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
}
2015-05-13 03:09:36 -03:00
int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Uncalibrated relative airpressure\n");
2012-08-21 23:19:51 -03:00
print_hit_enter();
2016-05-23 22:31:53 -03:00
init_barometer(true);
2012-08-21 23:19:51 -03:00
while(1) {
hal.scheduler->delay(100);
2015-01-05 00:56:18 -04:00
barometer.update();
if (!barometer.healthy()) {
cliSerial->println("not healthy");
} else {
cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n",
(double)barometer.get_altitude(),
(double)barometer.get_pressure(),
(double)barometer.get_temperature());
}
if(cliSerial->available() > 0) {
2012-08-21 23:19:51 -03:00
return (0);
}
}
}
2015-05-13 03:09:36 -03:00
void Plane::print_enabled(bool b)
{
if (b) {
cliSerial->printf("en");
2015-05-13 03:09:36 -03:00
} else {
cliSerial->printf("dis");
2015-05-13 03:09:36 -03:00
}
cliSerial->printf("abled\n");
2015-05-13 03:09:36 -03:00
}
#endif // CLI_ENABLED