Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

adapt mainline APM code to AP_Param

This commit is contained in:
Andrew Tridgell 2012-02-12 19:20:56 +11:00
parent 95e107ab75
commit bc0ed62aa9
5 changed files with 60 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/ArduPlane.pde
View File

@ -150,7 +150,7 @@ static AP_Baro_BMP085 barometer(false);
static AP_Baro_MS5611 barometer; static AP_Baro_MS5611 barometer;
#endif #endif
static AP_Compass_HMC5843 compass(Parameters::k_param_compass); static AP_Compass_HMC5843 compass;
#endif #endif
// real GPS selection // real GPS selection
@ -184,7 +184,7 @@ AP_GPS_None g_gps_driver(NULL);
# else # else
AP_InertialSensor_Oilpan ins( &adc ); AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE #endif // CONFIG_IMU_TYPE
AP_IMU_INS imu( &ins, Parameters::k_param_IMU_calibration ); AP_IMU_INS imu( &ins );
AP_DCM dcm(&imu, g_gps); AP_DCM dcm(&imu, g_gps);
#elif HIL_MODE == HIL_MODE_SENSORS #elif HIL_MODE == HIL_MODE_SENSORS
@ -217,8 +217,8 @@ AP_TimerProcess timer_scheduler;
// GCS selection // GCS selection
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// //
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0); GCS_MAVLINK gcs0;
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); GCS_MAVLINK gcs3;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// PITOT selection // PITOT selection

11
ArduPlane/GCS.h
View File

@ -108,7 +108,7 @@ protected:
class GCS_MAVLINK : public GCS_Class class GCS_MAVLINK : public GCS_Class
{ {
public: public:
GCS_MAVLINK(AP_Var::Key key); GCS_MAVLINK();
void update(void); void update(void);
void init(FastSerial *port); void init(FastSerial *port);
void send_message(enum ap_message id); void send_message(enum ap_message id);
@ -118,13 +118,16 @@ public:
void queued_param_send(); void queued_param_send();
void queued_waypoint_send(); void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
private: private:
void handleMessage(mavlink_message_t * msg); void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations /// Perform queued sending operations
/// ///
AP_Param *_queued_parameter; ///< next parameter to be sent in queue
AP_Var *_queued_parameter; ///< next parameter to be sent in queue enum ap_var_type _queued_parameter_type; ///< type of the next parameter
uint32_t _queued_parameter_token; ///AP_Param token for next() call
uint16_t _queued_parameter_index; ///< next queued parameter's index uint16_t _queued_parameter_index; ///< next queued parameter's index
uint16_t _queued_parameter_count; ///< saved count of parameters for queued send uint16_t _queued_parameter_count; ///< saved count of parameters for queued send
@ -139,7 +142,6 @@ private:
uint16_t _count_parameters(); ///< count reportable parameters uint16_t _count_parameters(); ///< count reportable parameters
uint16_t _parameter_count; ///< cache of reportable parameters uint16_t _parameter_count; ///< cache of reportable parameters
AP_Var *_find_parameter(uint16_t index); ///< find a reportable parameter by index
mavlink_channel_t chan; mavlink_channel_t chan;
uint16_t packet_drops; uint16_t packet_drops;
@ -163,7 +165,6 @@ private:
uint16_t waypoint_receive_timeout; // milliseconds uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates // data stream rates
AP_Var_group _group;
AP_Int16 streamRateRawSensors; AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus; AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels; AP_Int16 streamRateRCChannels;

131
ArduPlane/GCS_Mavlink.pde
View File

@ -729,28 +729,24 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
} }
} }
const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
AP_GROUPINFO("RAW_SENS", GCS_MAVLINK, streamRateRawSensors),
AP_GROUPINFO("EXT_STAT", GCS_MAVLINK, streamRateExtendedStatus),
AP_GROUPINFO("RC_CHAN", GCS_MAVLINK, streamRateRCChannels),
AP_GROUPINFO("RAW_CTRL", GCS_MAVLINK, streamRateRawController),
AP_GROUPINFO("POSITION", GCS_MAVLINK, streamRatePosition),
AP_GROUPINFO("EXTRA1", GCS_MAVLINK, streamRateExtra1),
AP_GROUPINFO("EXTRA2", GCS_MAVLINK, streamRateExtra2),
AP_GROUPINFO("EXTRA3", GCS_MAVLINK, streamRateExtra3),
AP_GROUPEND
};
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),
// parameters GCS_MAVLINK::GCS_MAVLINK() :
// note, all values not explicitly initialised here are zeroed packet_drops(0),
waypoint_send_timeout(1000), // 1 second waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second waypoint_receive_timeout(1000) // 1 second
// stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
// AP_VAR //ref //index, default, name
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{ {
} }
void void
@ -1113,12 +1109,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_ACTION_STORAGE_READ: case MAV_ACTION_STORAGE_READ:
AP_Var::load_all(); // we load all variables at startup, and
// save on each mavlink set
result=1; result=1;
break; break;
case MAV_ACTION_STORAGE_WRITE: case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all(); // this doesn't make any sense, as we save
// all settings as they come in
result=1; result=1;
break; break;
@ -1408,7 +1406,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// Start sending parameters - next call to ::update will kick the first one out // Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Var::first(); _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0; _queued_parameter_index = 0;
_queued_parameter_count = _count_parameters(); _queued_parameter_count = _count_parameters();
break; break;
@ -1703,8 +1701,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_SET: case MAVLINK_MSG_ID_PARAM_SET:
{ {
AP_Var *vp; AP_Param *vp;
AP_Meta_class::Type_id var_type; enum ap_var_type var_type;
// decode // decode
mavlink_param_set_t packet; mavlink_param_set_t packet;
@ -1720,7 +1718,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
key[ONBOARD_PARAM_NAME_LENGTH] = 0; key[ONBOARD_PARAM_NAME_LENGTH] = 0;
// find the requested parameter // find the requested parameter
vp = AP_Var::find(key); vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan !isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf !isinf(packet.param_value)) { // not inf
@ -1730,21 +1728,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// next lower integer value. // next lower integer value.
float rounding_addition = 0.01; float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
// handle variables with standard type IDs // handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) { if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(packet.param_value); ((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_float16) { } else if (var_type == AP_PARAM_INT32) {
((AP_Float16 *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_Var::k_typeid_int16) { } else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_Var::k_typeid_int8) { } else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else { } else {
@ -1759,8 +1752,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_param_value_send( mavlink_msg_param_value_send(
chan, chan,
key, key,
vp->cast_to_float(), vp->cast_to_float(var_type),
mav_var_type(vp->meta_type_id()), mav_var_type(var_type),
_count_parameters(), _count_parameters(),
-1); // XXX we don't actually know what its index is... -1); // XXX we don't actually know what its index is...
} }
@ -1975,44 +1968,17 @@ GCS_MAVLINK::_count_parameters()
{ {
// if we haven't cached the parameter count yet... // if we haven't cached the parameter count yet...
if (0 == _parameter_count) { if (0 == _parameter_count) {
AP_Var *vp; AP_Param *vp;
uint32_t token;
vp = AP_Var::first(); vp = AP_Param::first(&token, NULL);
do { do {
// if a parameter responds to cast_to_float then we are going to be able to report it _parameter_count++;
if (!isnan(vp->cast_to_float())) { } while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
_parameter_count++;
}
} while (NULL != (vp = vp->next()));
} }
return _parameter_count; return _parameter_count;
} }
AP_Var *
GCS_MAVLINK::_find_parameter(uint16_t index)
{
AP_Var *vp;
vp = AP_Var::first();
while (NULL != vp) {
// if the parameter is reportable
if (!(isnan(vp->cast_to_float()))) {
// if we have counted down to the index we want
if (0 == index) {
// return the parameter
return vp;
}
// count off this parameter, as it is reportable but not
// the one we want
index--;
}
// and move to the next parameter
vp = vp->next();
}
return NULL;
}
/** /**
* @brief Send the next pending parameter, called from deferred message * @brief Send the next pending parameter, called from deferred message
* handling code * handling code
@ -2023,29 +1989,28 @@ GCS_MAVLINK::queued_param_send()
// Check to see if we are sending parameters // Check to see if we are sending parameters
if (NULL == _queued_parameter) return; if (NULL == _queued_parameter) return;
AP_Var *vp; AP_Param *vp;
float value; float value;
// copy the current parameter and prepare to move to the next // copy the current parameter and prepare to move to the next
vp = _queued_parameter; vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
// if the parameter can be cast to float, report it here and break out of the loop // if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float(); value = vp->cast_to_float(_queued_parameter_type);
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send( char param_name[ONBOARD_PARAM_NAME_LENGTH];
chan, vp->copy_name(param_name, sizeof(param_name));
param_name,
value,
mav_var_type(vp->meta_type_id()),
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter_index++; mavlink_msg_param_value_send(
} chan,
param_name,
value,
mav_var_type(_queued_parameter_type),
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index++;
} }
/** /**

2
ArduPlane/setup.pde
View File

@ -85,7 +85,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
if (('y' != c) && ('Y' != c)) if (('y' != c) && ('Y' != c))
return(-1); return(-1);
AP_Var::erase_all(); AP_Param::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot() //default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()

23
ArduPlane/system.pde
View File

@ -128,28 +128,7 @@ static void init_ardupilot()
// //
// Check the EEPROM format version before loading any parameters from EEPROM. // Check the EEPROM format version before loading any parameters from EEPROM.
// //
load_parameters();
if (!g.format_version.load() ||
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
AP_Var::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"),
AP_Var::get_memory_use(), (unsigned)g.num_resets);
}
// keep a record of how many resets have happened. This can be // keep a record of how many resets have happened. This can be
// used to detect in-flight resets // used to detect in-flight resets