diff --git a/Tools/VARTest/Makefile b/Tools/VARTest/Makefile index 5ebdddae17..6903d4580e 100644 --- a/Tools/VARTest/Makefile +++ b/Tools/VARTest/Makefile @@ -2,3 +2,6 @@ # Trivial makefile for building APM # include ../../libraries/AP_Common/Arduino.mk + +sitl: + make -f ../../libraries/Desktop/Desktop.mk diff --git a/Tools/VARTest/Parameters.h b/Tools/VARTest/Parameters.h index 441ef0049f..e2d9043e09 100644 --- a/Tools/VARTest/Parameters.h +++ b/Tools/VARTest/Parameters.h @@ -53,8 +53,8 @@ public: // 110: Telemetry control // - k_param_streamrates_port0 = 110, - k_param_streamrates_port3, + k_param_gcs0 = 110, // stream rates for port0 + k_param_gcs3, // stream rates for port3 k_param_sysid_this_mav, k_param_sysid_my_gcs, k_param_serial3_baud, @@ -435,7 +435,7 @@ public: airspeed_enabled (AIRSPEED_SENSOR), // PID controller initial P initial I initial D initial imax - //--------------------------------------------------------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------- pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), diff --git a/Tools/VARTest/Parameters.pde b/Tools/VARTest/Parameters.pde index 30edc67009..8875396e1e 100644 --- a/Tools/VARTest/Parameters.pde +++ b/Tools/VARTest/Parameters.pde @@ -1,8 +1,12 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* - Parameters.pde example for new variable scheme - Andrew Tridgell February 2012 + ArduPlane parameter definitions + + This firmware is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. */ #define GSCALAR(v, name) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v } @@ -11,8 +15,8 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(format_version, "FORMAT_VERSION"), - GSCALAR(software_type, "SOFTWARE_TYPE"), - GSCALAR(sysid_this_mav, "SYSID_THIS_MAV"), + GSCALAR(software_type, "SYSID_SW_TYPE"), + GSCALAR(sysid_this_mav, "SYSID_THISMAV"), GSCALAR(sysid_my_gcs, "SYSID_MYGCS"), GSCALAR(serial3_baud, "SERIAL3_BAUD"), GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP"), @@ -117,7 +121,12 @@ static const AP_Param::Info var_info[] PROGMEM = { GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID), // variables not in the g class which contain EEPROM saved variables - GOBJECT(compass, "COMPASS_", Compass) + GOBJECT(compass, "COMPASS_", Compass), +#if 0 + // VARTest doesn't have these + GOBJECT(gcs0, "SR0_", GCS_MAVLINK), + GOBJECT(gcs3, "SR3_", GCS_MAVLINK) +#endif }; @@ -137,8 +146,8 @@ static void load_parameters(void) g.format_version != Parameters::k_format_version) { // erase all parameters - Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); - delay(100); // wait for serial send + Serial.printf_P(PSTR("Firmware change (%u -> %u): erasing EEPROM...\n"), + g.format_version.get(), Parameters::k_format_version); AP_Param::erase_all(); // save the current format version diff --git a/Tools/VARTest/VARTest.pde b/Tools/VARTest/VARTest.pde index ce3821d0d2..cadb01ed4f 100644 --- a/Tools/VARTest/VARTest.pde +++ b/Tools/VARTest/VARTest.pde @@ -7,19 +7,48 @@ #include #include +#include #include // PID library #include // ArduPilot Mega RC Library #include // RC Channel Library #include // ArduPilot Mega Magnetometer Library +#include // Arduino SPI lib #include +#include +#include +#include +#include +#include +#include // Inertial Sensor (uncalibated IMU) Library +#include // ArduPilot Mega IMU Library +#include #include #include #include static Parameters g; + +static AP_ADC_ADS7844 adc; +static GPS *g_gps; +AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); +# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 + AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); +# else + AP_InertialSensor_Oilpan ins( &adc ); +#endif // CONFIG_IMU_TYPE +AP_IMU_INS imu( &ins ); +AP_DCM dcm(&imu, g_gps); + +Arduino_Mega_ISR_Registry isr_registry; +#ifdef DESKTOP_BUILD +AP_Compass_HIL compass; +#else static AP_Compass_HMC5843 compass; +#endif +AP_Baro_BMP085_HIL barometer; FastSerialPort0(Serial); +FastSerialPort1(Serial1); // GPS port #define SERIAL0_BAUD 115200 @@ -96,6 +125,10 @@ void test_variable(AP_Param *ap, enum ap_var_type type) type2 != type) { Debug("find failed"); } + if (strcmp(s, "FORMAT_VERSION") == 0) { + // don't wipe the version + return; + } switch (type) { case AP_PARAM_INT8: { AP_Int8 *v = (AP_Int8 *)ap;