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

Tools: Replay: pass a lambda function to LR_MsgHandler_PARM to set parameters 

This avoid duplication of parameter setting logic.
This commit is contained in:
Peter Barker 2015-10-26 17:37:16 +11:00 committed by Francisco Ferreira
parent d3a18e803e
commit d475b91564
4 changed files with 47 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Tools/Replay/LR_MsgHandler.cpp
View File

@ -379,7 +379,7 @@ void LR_MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
}
bool LR_MsgHandler::set_parameter(const char *name, float value)
bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
{
const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_TYPE", "EK2_ENABLE", "EK3_ENABLE"
"COMPASS_ORIENT", "COMPASS_ORIENT2",
@ -390,32 +390,8 @@ bool LR_MsgHandler::set_parameter(const char *name, float value)
return true;
}
}
enum ap_var_type var_type;
AP_Param *vp = AP_Param::find(name, &var_type);
if (vp == NULL) {
return false;
}
float old_value = 0;
if (var_type == AP_PARAM_FLOAT) {
old_value = ((AP_Float *)vp)->cast_to_float();
((AP_Float *)vp)->set(value);
} else if (var_type == AP_PARAM_INT32) {
old_value = ((AP_Int32 *)vp)->cast_to_float();
((AP_Int32 *)vp)->set(value);
} else if (var_type == AP_PARAM_INT16) {
old_value = ((AP_Int16 *)vp)->cast_to_float();
((AP_Int16 *)vp)->set(value);
} else if (var_type == AP_PARAM_INT8) {
old_value = ((AP_Int8 *)vp)->cast_to_float();
((AP_Int8 *)vp)->set(value);
} else {
// we don't support mavlink set on this parameter
return false;
}
if (fabsf(old_value - value) > 1.0e-12) {
::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
}
return true;
return _set_parameter_callback(name, value);
}
void LR_MsgHandler_PARM::process_message(uint8_t *msg)

14
Tools/Replay/LR_MsgHandler.h
View File

@ -2,13 +2,14 @@
#include "MsgHandler.h"
#include <functional>
class LR_MsgHandler : public MsgHandler {
public:
LR_MsgHandler(struct log_Format &f,
DataFlash_Class &_dataflash,
uint64_t &last_timestamp_usec);
virtual void process_message(uint8_t *msg) = 0;
bool set_parameter(const char *name, float value);
// state for CHEK message
struct CheckState {
@ -430,9 +431,18 @@ private:
class LR_MsgHandler_PARM : public LR_MsgHandler
{
public:
LR_MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec) : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
LR_MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
const std::function<bool(const char *name, const float)>&set_parameter_callback) :
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
_set_parameter_callback(set_parameter_callback)
{};
virtual void process_message(uint8_t *msg);
private:
bool set_parameter(const char *name, const float value);
const std::function<bool(const char *name, const float)>_set_parameter_callback;
};
class LR_MsgHandler_PM : public LR_MsgHandler

39
Tools/Replay/LogReader.cpp
View File

@ -85,8 +85,6 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
}
}
LR_MsgHandler_PARM *parameter_handler;
/*
messages which we will be generating, so should be discarded
*/
@ -171,9 +169,12 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
// map from format name to a parser subclass:
if (streq(name, "PARM")) {
parameter_handler = new LR_MsgHandler_PARM(formats[f.type], dataflash,
last_timestamp_usec);
msgparser[f.type] = parameter_handler;
msgparser[f.type] = new LR_MsgHandler_PARM
(formats[f.type], dataflash,
last_timestamp_usec,
[this](const char *xname, const float xvalue) {
return set_parameter(xname, xvalue);
});
} else if (streq(name, "GPS")) {
msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type],
dataflash,
@ -323,14 +324,34 @@ bool LogReader::wait_type(const char *wtype)
return true;
}
bool LogReader::set_parameter(const char *name, float value)
{
if (parameter_handler == NULL) {
::printf("No parameter format message found");
enum ap_var_type var_type;
AP_Param *vp = AP_Param::find(name, &var_type);
if (vp == NULL) {
return false;
}
return parameter_handler->set_parameter(name, value);
float old_value = 0;
if (var_type == AP_PARAM_FLOAT) {
old_value = ((AP_Float *)vp)->cast_to_float();
((AP_Float *)vp)->set(value);
} else if (var_type == AP_PARAM_INT32) {
old_value = ((AP_Int32 *)vp)->cast_to_float();
((AP_Int32 *)vp)->set(value);
} else if (var_type == AP_PARAM_INT16) {
old_value = ((AP_Int16 *)vp)->cast_to_float();
((AP_Int16 *)vp)->set(value);
} else if (var_type == AP_PARAM_INT8) {
old_value = ((AP_Int8 *)vp)->cast_to_float();
((AP_Int8 *)vp)->set(value);
} else {
// we don't support mavlink set on this parameter
return false;
}
if (fabsf(old_value - value) > 1.0e-12) {
::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
}
return true;
}
/*

11
Tools/Replay/Replay.cpp
View File

@ -558,17 +558,10 @@ void Replay::set_ins_update_rate(uint16_t _update_rate) {
}
void Replay::inhibit_gyro_cal() {
// swiped from LR_MsgHandler.cpp; until we see PARM messages, we
// don't have a PARM handler available to set parameters.
enum ap_var_type var_type;
AP_Param *vp = AP_Param::find("INS_GYR_CAL", &var_type);
if (vp == NULL) {
::fprintf(stderr, "No GYR_CAL parameter found\n");
if (!logreader.set_parameter("INS_GYR_CAL", AP_InertialSensor::GYRO_CAL_NEVER)) {
::fprintf(stderr, "Failed to set GYR_CAL parameter\n");
abort();
}
((AP_Float *)vp)->set(AP_InertialSensor::GYRO_CAL_NEVER);
// logreader.set_parameter("GYR_CAL", AP_InertialSensor::GYRO_CAL_NEVER);
}
/*