ardupilot/Tools/ArduTracker/test.pde

520 lines
13 KiB
Plaintext
Raw Normal View History

2011-09-08 22:31:32 -03:00
// -*- 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