update VARTest to use exact variables from ArduPlane

This commit is contained in:
Andrew Tridgell 2012-02-13 09:55:39 +11:00
parent 9c5a5473ab
commit 31d28e2181
4 changed files with 55 additions and 10 deletions

View File

@ -2,3 +2,6 @@
# Trivial makefile for building APM
#
include ../../libraries/AP_Common/Arduino.mk
sitl:
make -f ../../libraries/Desktop/Desktop.mk

View File

@ -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),

View File

@ -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

View File

@ -7,19 +7,48 @@
#include <FastSerial.h>
#include <AP_Common.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <PID.h> // PID library
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // RC Channel Library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <SPI.h> // Arduino SPI lib
#include <I2C.h>
#include <DataFlash.h>
#include <AP_DCM.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_GPS.h>
#include <AP_Math.h>
#include <config.h>
#include <Parameters.h>
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;