mirror of https://github.com/ArduPilot/ardupilot
520 lines
13 KiB
Plaintext
520 lines
13 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
// These are function definitions so the Menu can be constructed before the functions
|
|
// are defined below. Order matters to the compiler.
|
|
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_gyro(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
|
|
|
|
// 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
|
|
const struct Menu::command test_menu_commands[] PROGMEM = {
|
|
{"radio", test_radio},
|
|
{"battery", test_battery},
|
|
{"relay", test_relay},
|
|
{"waypoints", test_wp},
|
|
{"xbee", test_xbee},
|
|
{"eedump", test_eedump},
|
|
{"modeswitch", test_modeswitch},
|
|
{"dipswitches", test_dipswitches},
|
|
|
|
// Tests below here are for hardware sensors only present
|
|
// when real sensors are attached or they are emulated
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
{"adc", test_adc},
|
|
{"gps", test_gps},
|
|
{"rawgps", test_rawgps},
|
|
{"imu", test_imu},
|
|
{"gyro", test_gyro},
|
|
{"airspeed", test_airspeed},
|
|
{"airpressure", test_pressure},
|
|
{"compass", test_mag},
|
|
#elif HIL_MODE == HIL_MODE_SENSORS
|
|
{"adc", test_adc},
|
|
{"gps", test_gps},
|
|
{"imu", test_imu},
|
|
{"gyro", test_gyro},
|
|
{"compass", test_mag},
|
|
#elif HIL_MODE == HIL_MODE_ATTITUDE
|
|
#endif
|
|
|
|
};
|
|
|
|
// A Macro to create the Menu
|
|
MENU(test_menu, "test", test_menu_commands);
|
|
|
|
int8_t
|
|
test_mode(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Serial.printf_P(PSTR("Test Mode\n\n"));
|
|
test_menu.run();
|
|
}
|
|
|
|
void print_hit_enter()
|
|
{
|
|
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
|
|
}
|
|
|
|
static int8_t
|
|
test_eedump(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
int i, j;
|
|
|
|
// hexdump the EEPROM
|
|
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
|
|
Serial.printf_P(PSTR("%04x:"), i);
|
|
for (j = 0; j < 16; j++)
|
|
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
|
|
Serial.println();
|
|
}
|
|
return(0);
|
|
}
|
|
|
|
static int8_t
|
|
test_radio(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
#if THROTTLE_REVERSE == 1
|
|
Serial.println("Throttle is reversed in config: ");
|
|
delay(1000);
|
|
#endif
|
|
|
|
// read the radio to set trims
|
|
// ---------------------------
|
|
trim_radio();
|
|
|
|
while(1){
|
|
delay(20);
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
// ----------------------------------------------------------
|
|
read_radio();
|
|
update_servo_switches();
|
|
|
|
servo_out[CH_ROLL] = reverse_roll * int(radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9;
|
|
servo_out[CH_PITCH] = reverse_pitch * int(radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9;
|
|
servo_out[CH_RUDDER] = reverse_rudder * int(radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9;
|
|
|
|
// write out the servo PWM values
|
|
// ------------------------------
|
|
set_servos_4();
|
|
|
|
|
|
for(int y = 4; y < 8; y++) {
|
|
radio_out[y] = constrain(radio_in[y], radio_min(y), radio_max(y));
|
|
APM_RC.OutputCh(y, radio_out[y]); // send to Servos
|
|
}
|
|
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\t"), radio_in[CH_1], radio_in[CH_2], radio_in[CH_3], radio_in[CH_4], radio_in[CH_5], radio_in[CH_6], radio_in[CH_7], radio_in[CH_8]);
|
|
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (servo_out[CH_ROLL]/100), (servo_out[CH_PITCH]/100), servo_out[CH_THROTTLE], (servo_out[CH_RUDDER]/100));
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
static int8_t
|
|
test_battery(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
#if BATTERY_EVENT == 1
|
|
for (int i = 0; i < 20; i++){
|
|
delay(20);
|
|
read_battery();
|
|
}
|
|
Serial.printf_P(PSTR("Volts: 1:"));
|
|
Serial.print(battery_voltage1, 4);
|
|
Serial.print(" 2:");
|
|
Serial.print(battery_voltage2, 4);
|
|
Serial.print(" 3:");
|
|
Serial.print(battery_voltage3, 4);
|
|
Serial.print(" 4:");
|
|
Serial.println(battery_voltage4, 4);
|
|
#else
|
|
Serial.printf_P(PSTR("Not enabled\n"));
|
|
|
|
#endif
|
|
return (0);
|
|
}
|
|
|
|
|
|
static int8_t
|
|
test_relay(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
Serial.println("Relay on");
|
|
relay_on();
|
|
delay(3000);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
|
|
Serial.println("Relay off");
|
|
relay_off();
|
|
delay(3000);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
void
|
|
test_wp_print(struct Location *cmd, byte index)
|
|
{
|
|
Serial.print("command #: ");
|
|
Serial.print(index, DEC);
|
|
Serial.print(" id: ");
|
|
Serial.print(cmd->id, DEC);
|
|
Serial.print(" p1: ");
|
|
Serial.print(cmd->p1, DEC);
|
|
Serial.print(" p2: ");
|
|
Serial.print(cmd->alt, DEC);
|
|
Serial.print(" p3: ");
|
|
Serial.print(cmd->lat, DEC);
|
|
Serial.print(" p4: ");
|
|
Serial.println(cmd->lng, DEC);
|
|
}
|
|
|
|
static int8_t
|
|
test_wp(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
delay(1000);
|
|
ground_alt = 0;
|
|
//read_EEPROM_waypoint_info();
|
|
|
|
// save the alitude above home option
|
|
if(get(PARAM_CONFIG) & HOLD_ALT_ABOVE_HOME){
|
|
Serial.printf_P(PSTR("Hold altitude of %ldm\n"), get(PARAM_ALT_HOLD_HOME)/100);
|
|
}else{
|
|
Serial.printf_P(PSTR("Hold current altitude\n"));
|
|
}
|
|
|
|
Serial.printf_P(PSTR("%d waypoints\n"), get(PARAM_WP_TOTAL));
|
|
Serial.printf_P(PSTR("Hit radius: %d\n"), get(PARAM_WP_RADIUS));
|
|
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), get(PARAM_LOITER_RADIUS));
|
|
|
|
for(byte i = 0; i <= get(PARAM_WP_TOTAL); i++){
|
|
struct Location temp = get_wp_with_index(i);
|
|
test_wp_print(&temp, i);
|
|
}
|
|
|
|
return (0);
|
|
}
|
|
|
|
static int8_t
|
|
test_xbee(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
|
|
while(1){
|
|
delay(250);
|
|
// Timeout set high enough for X-CTU RSSI Calc over XBee @ 115200
|
|
Serial3.printf_P(PSTR("0123456789:;<=>?@ABCDEFGHIJKLMNO\n"));
|
|
//Serial.print("X");
|
|
// Default 32bit data from X-CTU Range Test
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
Serial.print("Control CH ");
|
|
Serial.println(FLIGHT_MODE_CHANNEL, DEC);
|
|
|
|
while(1){
|
|
delay(20);
|
|
byte switchPosition = readSwitch();
|
|
if (oldSwitchPosition != switchPosition){
|
|
Serial.printf_P(PSTR("Position %d\n"), switchPosition);
|
|
oldSwitchPosition = switchPosition;
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_dipswitches(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(100);
|
|
update_servo_switches();
|
|
if (mix_mode == 0) {
|
|
Serial.print("Mix:standard ");
|
|
Serial.print("\t reverse_roll: ");
|
|
Serial.print(reverse_roll, DEC);
|
|
Serial.print("\t reverse_pitch: ");
|
|
Serial.print(reverse_pitch, DEC);
|
|
Serial.print("\t reverse_rudder: ");
|
|
Serial.println(reverse_rudder, DEC);
|
|
} else {
|
|
Serial.print("Mix:elevons ");
|
|
Serial.print("\t reverse_elevons: ");
|
|
Serial.print(reverse_elevons, DEC);
|
|
Serial.print("\t reverse_ch1_elevon: ");
|
|
Serial.print(reverse_ch1_elevon, DEC);
|
|
Serial.print("\t reverse_ch2_elevon: ");
|
|
Serial.println(reverse_ch2_elevon, DEC);
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
// tests in this section are for real sensors or sensors that have been simulated
|
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
|
|
static int8_t
|
|
test_adc(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
adc.Init();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("ADC\n"));
|
|
delay(1000);
|
|
|
|
while(1){
|
|
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i));
|
|
Serial.println();
|
|
delay(100);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_gps(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(333);
|
|
|
|
// Blink GPS LED if we don't have a fix
|
|
// ------------------------------------
|
|
update_GPS_light();
|
|
|
|
gps.update();
|
|
|
|
if (gps.new_data){
|
|
Serial.print("Lat:");
|
|
Serial.print((float)gps.latitude/10000000, 10);
|
|
Serial.print(" Lon:");
|
|
Serial.print((float)gps.longitude/10000000, 10);
|
|
Serial.printf_P(PSTR("alt %ldm, #sats: %d\n"), gps.altitude/100, gps.num_sats);
|
|
}else{
|
|
Serial.print(".");
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_imu(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
//Serial.printf_P(PSTR("Calibrating."));
|
|
|
|
imu.init_gyro();
|
|
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(20);
|
|
if (millis() - fast_loopTimer > 19) {
|
|
deltaMiliSeconds = millis() - fast_loopTimer;
|
|
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
|
|
fast_loopTimer = millis();
|
|
|
|
// IMU
|
|
// ---
|
|
dcm.update_DCM(G_Dt);
|
|
|
|
#if MAGNETOMETER == 1
|
|
medium_loopCounter++;
|
|
if(medium_loopCounter == 5){
|
|
compass.read(); // Read magnetometer
|
|
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
|
|
medium_loopCounter = 0;
|
|
}
|
|
#endif
|
|
|
|
// We are using the IMU
|
|
// ---------------------
|
|
Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
|
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
static int8_t
|
|
test_gyro(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
adc.Init();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("Gyro | Accel\n"));
|
|
delay(1000);
|
|
|
|
while(1){
|
|
Vector3f gyros = imu.get_gyro();
|
|
Vector3f accels = imu.get_accel();
|
|
Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)gyros.x, (int)gyros.y, (int)gyros.z, (int)accels.x, (int)accels.y, (int)accels.z);
|
|
delay(100);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_mag(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
#if MAGNETOMETER == DISABLED
|
|
Serial.printf_P(PSTR("Compass disabled\n"));
|
|
return (0);
|
|
#else
|
|
print_hit_enter();
|
|
|
|
while(1){
|
|
delay(250);
|
|
compass.read();
|
|
compass.calculate(0,0);
|
|
Serial.printf_P(PSTR("Heading: ("));
|
|
Serial.print(ToDeg(compass.heading));
|
|
Serial.printf_P(PSTR(") XYZ: ("));
|
|
Serial.print(compass.mag_x);
|
|
Serial.print(comma);
|
|
Serial.print(compass.mag_y);
|
|
Serial.print(comma);
|
|
Serial.print(compass.mag_z);
|
|
Serial.println(")");
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
// real sensors that have not been simulated yet go here
|
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED
|
|
|
|
static int8_t
|
|
test_airspeed(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
#if AIRSPEED_SENSOR == DISABLED
|
|
Serial.printf_P(PSTR("airspeed disabled\n"));
|
|
return (0);
|
|
#else
|
|
print_hit_enter();
|
|
zero_airspeed();
|
|
|
|
while(1){
|
|
delay(20);
|
|
read_airspeed();
|
|
Serial.printf_P(PSTR("%dm/s\n"),airspeed/100);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
static int8_t
|
|
test_pressure(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
uint32_t sum = 0;
|
|
Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
|
|
Serial.printf_P(PSTR("Note - the altitude displayed is relative to the start of this test\n"));
|
|
print_hit_enter();
|
|
Serial.printf_P(PSTR("\nCalibrating....\n"));
|
|
for (int i=1;i<301;i++) {
|
|
read_airpressure();
|
|
if(i>200)sum += abs_press_filt;
|
|
delay(10);
|
|
}
|
|
abs_press_gnd = (double)sum/100.0;
|
|
|
|
ground_alt = 0;
|
|
while(1){
|
|
delay(100);
|
|
read_airpressure();
|
|
//Serial.printf_P(PSTR("Alt: %dm, Raw: %d\n"), press_alt / 100, abs_press_filt); // Someone needs to fix the formatting here for long integers
|
|
Serial.print("Altitude: ");
|
|
Serial.print(press_alt/100.0,2);
|
|
Serial.print(" Raw pressure value: ");
|
|
Serial.println(abs_press_filt);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_rawgps(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
if(Serial1.available() > 0) // Ok, let me see, the buffer is empty?
|
|
{
|
|
delay(60); // wait for it all
|
|
while(Serial1.available() > 0){
|
|
byte incoming = Serial1.read();
|
|
//Serial.print(incoming,DEC);
|
|
Serial.print(incoming, BYTE); // will output Hex values
|
|
//Serial.print(",");
|
|
}
|
|
Serial.println(" ");
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
#endif // HIL_MODE == HIL_MODE_DISABLED
|