diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 555a214ea3..8859af1903 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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 diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index dfee2c77cb..0b492e243b 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -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; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 12c1d2e1a2..a6597413d7 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -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())); + _parameter_count++; + } 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 - vp->copy_name(param_name, sizeof(param_name)); + value = vp->cast_to_float(_queued_parameter_type); - mavlink_msg_param_value_send( - chan, - param_name, - value, - mav_var_type(vp->meta_type_id()), - _queued_parameter_count, - _queued_parameter_index); + char param_name[ONBOARD_PARAM_NAME_LENGTH]; + vp->copy_name(param_name, sizeof(param_name)); - _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++; } /** diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 49dd6f9ca7..7de58d651c 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -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() diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 4b37de48d3..37d8fb17a2 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -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