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;
#endif
static AP_Compass_HMC5843 compass(Parameters::k_param_compass);
static AP_Compass_HMC5843 compass;
#endif
// real GPS selection
@ -184,7 +184,7 @@ AP_GPS_None g_gps_driver(NULL);
# else
AP_InertialSensor_Oilpan ins( &adc );
#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);
#elif HIL_MODE == HIL_MODE_SENSORS
@ -217,8 +217,8 @@ AP_TimerProcess timer_scheduler;
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0);
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
GCS_MAVLINK gcs0;
GCS_MAVLINK gcs3;
////////////////////////////////////////////////////////////////////////////////
// PITOT selection

11
ArduPlane/GCS.h
View File

@ -108,7 +108,7 @@ protected:
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK(AP_Var::Key key);
GCS_MAVLINK();
void update(void);
void init(FastSerial *port);
void send_message(enum ap_message id);
@ -118,13 +118,16 @@ public:
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
AP_Param *_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_count; ///< saved count of parameters for queued send
@ -139,7 +142,6 @@ private:
uint16_t _count_parameters(); ///< count 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;
uint16_t packet_drops;
@ -163,7 +165,6 @@ private:
uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates
AP_Var_group _group;
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;

113
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
// note, all values not explicitly initialised here are zeroed
waypoint_send_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"))
GCS_MAVLINK::GCS_MAVLINK() :
packet_drops(0),
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000) // 1 second
{
}
void
@ -1113,12 +1109,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
// we load all variables at startup, and
// save on each mavlink set
result=1;
break;
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;
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
_queued_parameter = AP_Var::first();
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
break;
@ -1703,8 +1701,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_SET:
{
AP_Var *vp;
AP_Meta_class::Type_id var_type;
AP_Param *vp;
enum ap_var_type var_type;
// decode
mavlink_param_set_t packet;
@ -1720,7 +1718,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
key[ONBOARD_PARAM_NAME_LENGTH] = 0;
// find the requested parameter
vp = AP_Var::find(key);
vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
@ -1730,21 +1728,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// next lower integer value.
float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
// 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);
} else if (var_type == AP_Var::k_typeid_float16) {
((AP_Float16 *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) {
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -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;
((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;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else {
@ -1759,8 +1752,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_param_value_send(
chan,
key,
vp->cast_to_float(),
mav_var_type(vp->meta_type_id()),
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-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 (0 == _parameter_count) {
AP_Var *vp;
AP_Param *vp;
uint32_t token;
vp = AP_Var::first();
vp = AP_Param::first(&token, NULL);
do {
// if a parameter responds to cast_to_float then we are going to be able to report it
if (!isnan(vp->cast_to_float())) {
_parameter_count++;
}
} while (NULL != (vp = vp->next()));
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
}
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
* handling code
@ -2023,29 +1989,28 @@ GCS_MAVLINK::queued_param_send()
// Check to see if we are sending parameters
if (NULL == _queued_parameter) return;
AP_Var *vp;
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
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
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
value = vp->cast_to_float(_queued_parameter_type);
char param_name[ONBOARD_PARAM_NAME_LENGTH];
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(
chan,
param_name,
value,
mav_var_type(vp->meta_type_id()),
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))
return(-1);
AP_Var::erase_all();
AP_Param::erase_all();
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()

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.
//
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);
}
load_parameters();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets