You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
357 lines
8.4 KiB
357 lines
8.4 KiB
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- |
|
|
|
/* |
|
new variable scheme |
|
Andrew Tridgell February 2012 |
|
*/ |
|
|
|
#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_AHRS.h> |
|
#include <AP_ADC.h> |
|
#include <AP_Baro.h> |
|
#include <Filter.h> |
|
#include <GCS_MAVLink.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 <GCS_MAVLink.h> |
|
#include <config.h> |
|
#include <Parameters.h> |
|
|
|
#if 0 |
|
#include <AP_Declination.h> |
|
#endif |
|
|
|
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_AHRS_DCM ahrs(&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 |
|
|
|
#define Debug(fmt, args...) Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args) |
|
|
|
|
|
void setup() { |
|
Serial.begin(SERIAL0_BAUD, 128, 128); |
|
|
|
load_parameters(); |
|
|
|
// show some sizes |
|
Serial.printf_P(PSTR("sizeof(RC_Channel)=%u\n"), (unsigned)sizeof(RC_Channel)); |
|
Serial.printf_P(PSTR("sizeof(g)=%u\n"), (unsigned)sizeof(g)); |
|
Serial.printf_P(PSTR("sizeof(g.throttle_min)=%u\n"), (unsigned)sizeof(g.throttle_min)); |
|
Serial.printf_P(PSTR("sizeof(g.channel_roll)=%u\n"), (unsigned)sizeof(g.channel_roll)); |
|
Serial.printf_P(PSTR("throttle_max now: %u\n"), (unsigned)g.throttle_max); |
|
|
|
// some ad-hoc testing |
|
|
|
// try set interfaces |
|
g.throttle_min.set(g.throttle_min+1); |
|
g.throttle_min.save(); |
|
g.throttle_min.set_and_save(g.throttle_min+1); |
|
|
|
Serial.printf_P(PSTR("throttle_min now: %u\n"), (unsigned)g.throttle_min); |
|
|
|
// find a variable by name |
|
AP_Param *vp; |
|
enum ap_var_type type; |
|
vp = AP_Param::find("RLL2SRV_P", &type); |
|
((AP_Float *)vp)->set(23); |
|
|
|
Serial.printf_P(PSTR("RLL2SRV_P=%f\n"), |
|
g.pidServoRoll.kP()); |
|
|
|
char s[AP_MAX_NAME_SIZE+1]; |
|
|
|
g.throttle_min.copy_name(s, sizeof(s)); |
|
s[AP_MAX_NAME_SIZE] = 0; |
|
Serial.printf_P(PSTR("THROTTLE_MIN.copy_name()->%s\n"), s); |
|
|
|
g.channel_roll.radio_min.copy_name(s, sizeof(s)); |
|
s[AP_MAX_NAME_SIZE] = 0; |
|
Serial.printf_P(PSTR("RC1_MIN.copy_name()->%s %p\n"), s, &g.channel_roll.radio_min); |
|
|
|
Vector3f ofs; |
|
ofs = compass.get_offsets(); |
|
Serial.printf_P(PSTR("Compass: %f %f %f\n"), |
|
ofs.x, ofs.y, ofs.z); |
|
ofs.x += 1.1; |
|
ofs.y += 1.2; |
|
ofs.z += 1.3; |
|
compass.set_offsets(ofs); |
|
compass.save_offsets(); |
|
Serial.printf_P(PSTR("Compass: %f %f %f\n"), |
|
ofs.x, ofs.y, ofs.z); |
|
|
|
test_vector3f(); |
|
|
|
// full testing of all variables |
|
AP_Param::ParamToken token; |
|
for (AP_Param *ap = AP_Param::first(&token, &type); |
|
ap; |
|
ap=AP_Param::next(&token, &type)) { |
|
test_variable(ap, type); |
|
} |
|
|
|
AP_Param::show_all(); |
|
|
|
Serial.println_P(PSTR("All done.")); |
|
} |
|
|
|
void loop() |
|
{ |
|
} |
|
|
|
// test vector3f handling |
|
void test_vector3f(void) |
|
{ |
|
enum ap_var_type type; |
|
|
|
AP_Float *f; |
|
AP_Vector3f *v; |
|
Vector3f vec; |
|
|
|
v = (AP_Vector3f *)AP_Param::find("COMPASS_OFS", &type); |
|
|
|
f = (AP_Float *)AP_Param::find("COMPASS_OFS_X", &type); |
|
f->set_and_save(10); |
|
f = (AP_Float *)AP_Param::find("COMPASS_OFS_Y", &type); |
|
f->set_and_save(11); |
|
f = (AP_Float *)AP_Param::find("COMPASS_OFS_Z", &type); |
|
f->set_and_save(12); |
|
|
|
v->load(); |
|
|
|
vec = *v; |
|
Serial.printf_P(PSTR("vec %f %f %f\n"), |
|
vec.x, vec.y, vec.z); |
|
|
|
if (vec.x != 10 || |
|
vec.y != 11 || |
|
vec.z != 12) { |
|
Serial.printf_P(PSTR("wrong value for compass vector\n")); |
|
} |
|
} |
|
|
|
|
|
// test all interfaces for a variable |
|
void test_variable(AP_Param *ap, enum ap_var_type type) |
|
{ |
|
static int8_t value; |
|
char s[AP_MAX_NAME_SIZE+1]; |
|
|
|
value++; |
|
|
|
ap->copy_name(s, sizeof(s), type==AP_PARAM_FLOAT); |
|
Serial.printf_P(PSTR("Testing variable '%s' of type %u\n"), |
|
s, type); |
|
enum ap_var_type type2; |
|
if (AP_Param::find(s, &type2) != ap || |
|
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; |
|
if (sizeof(*v) != 1) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
v->set(value); |
|
if (!v->save()) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->get() != value) { |
|
Debug("wrong value %d %d", (int)v->get(), (int)value); |
|
} |
|
if (!v->set_and_save(value+1)) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->vtype != type) { |
|
Debug("wrong type"); |
|
} |
|
if (v->get() != value+1) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
if (*v != value+1) { |
|
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
*v = value+2; |
|
if (v->get() != value+2) { |
|
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2); |
|
} |
|
break; |
|
} |
|
case AP_PARAM_INT16: { |
|
AP_Int16 *v = (AP_Int16 *)ap; |
|
if (sizeof(*v) != 2) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
v->set(value); |
|
if (!v->save()) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->get() != value) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value); |
|
} |
|
if (!v->set_and_save(value+1)) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->vtype != type) { |
|
Debug("wrong type"); |
|
} |
|
if (v->get() != value+1) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
if (*v != value+1) { |
|
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
*v = value+2; |
|
if (v->get() != value+2) { |
|
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2); |
|
} |
|
break; |
|
} |
|
case AP_PARAM_INT32: { |
|
AP_Int32 *v = (AP_Int32 *)ap; |
|
if (sizeof(*v) != 4) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
v->set(value); |
|
if (!v->save()) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->get() != value) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value); |
|
} |
|
if (!v->set_and_save(value+1)) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->vtype != type) { |
|
Debug("wrong type"); |
|
} |
|
if (v->get() != value+1) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
if (*v != value+1) { |
|
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
*v = value+2; |
|
if (v->get() != value+2) { |
|
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2); |
|
} |
|
break; |
|
} |
|
case AP_PARAM_FLOAT: { |
|
AP_Float *v = (AP_Float *)ap; |
|
if (sizeof(*v) != 4) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
v->set(value); |
|
if (!v->save()) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->get() != value) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value); |
|
} |
|
if (!v->set_and_save(value+1)) { |
|
Debug("failed set_and_save"); |
|
} |
|
if (!v->load()) { |
|
Debug("failed load"); |
|
} |
|
if (v->vtype != type) { |
|
Debug("wrong type"); |
|
} |
|
if (v->get() != value+1) { |
|
Debug("wrong value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
if (*v != value+1) { |
|
Debug("wrong direct value %u %u", (unsigned)v->get(), value+1); |
|
} |
|
*v = value+2; |
|
if (v->get() != value+2) { |
|
Debug("wrong copy assignment value %u %u", (unsigned)v->get(), value+2); |
|
} |
|
break; |
|
} |
|
case AP_PARAM_VECTOR3F: { |
|
AP_Vector3f *v = (AP_Vector3f *)ap; |
|
if (sizeof(*v) != 12) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
break; |
|
} |
|
case AP_PARAM_VECTOR6F: { |
|
AP_Vector6f *v = (AP_Vector6f *)ap; |
|
if (sizeof(*v) != 24) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
break; |
|
} |
|
case AP_PARAM_MATRIX3F: { |
|
AP_Matrix3f *v = (AP_Matrix3f *)ap; |
|
if (sizeof(*v) != 36) { |
|
Debug("incorrect size %u", (unsigned)sizeof(*v)); |
|
} |
|
break; |
|
} |
|
default: |
|
break; |
|
} |
|
}
|
|
|