mirror of https://github.com/ArduPilot/ardupilot
Replay: cleanup unused code
This commit is contained in:
parent
509b9f5e63
commit
541d11ee05
|
@ -268,13 +268,6 @@ void LR_MsgHandler_REVH::process_message(uint8_t *msg)
|
||||||
bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
|
bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
|
||||||
{
|
{
|
||||||
const char *ignore_parms[] = {
|
const char *ignore_parms[] = {
|
||||||
// "GPS_TYPE",
|
|
||||||
// "AHRS_EKF_TYPE",
|
|
||||||
// "EK2_ENABLE",
|
|
||||||
// "EK3_ENABLE",
|
|
||||||
// "COMPASS_ORIENT",
|
|
||||||
// "COMPASS_ORIENT2",
|
|
||||||
// "COMPASS_ORIENT3",
|
|
||||||
"LOG_FILE_BUFSIZE",
|
"LOG_FILE_BUFSIZE",
|
||||||
"LOG_DISARMED"
|
"LOG_DISARMED"
|
||||||
};
|
};
|
||||||
|
@ -307,9 +300,5 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
|
||||||
require_field(msg, "Name", parameter_name, parameter_name_len);
|
require_field(msg, "Name", parameter_name, parameter_name_len);
|
||||||
|
|
||||||
float value = require_field_float(msg, "Value");
|
float value = require_field_float(msg, "Value");
|
||||||
// if (globals.no_params || replay.check_user_param(parameter_name)) {
|
|
||||||
// printf("Not changing %s to %f\n", parameter_name, value);
|
|
||||||
// } else {
|
|
||||||
set_parameter(parameter_name, value);
|
set_parameter(parameter_name, value);
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -18,17 +18,6 @@ public:
|
||||||
// like it.
|
// like it.
|
||||||
process_message(msg);
|
process_message(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
// state for CHEK message
|
|
||||||
struct CheckState {
|
|
||||||
uint64_t time_us;
|
|
||||||
Vector3f euler;
|
|
||||||
Location pos;
|
|
||||||
Vector3f velocity;
|
|
||||||
};
|
|
||||||
|
|
||||||
protected:
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
class LR_MsgHandler_RFRH : public LR_MsgHandler
|
class LR_MsgHandler_RFRH : public LR_MsgHandler
|
||||||
|
@ -274,7 +263,6 @@ class LR_MsgHandler_PARM : public LR_MsgHandler
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
LR_MsgHandler_PARM(log_Format &_f,
|
LR_MsgHandler_PARM(log_Format &_f,
|
||||||
uint64_t &_last_timestamp_usec,
|
|
||||||
const std::function<bool(const char *name, const float)>&set_parameter_callback) :
|
const std::function<bool(const char *name, const float)>&set_parameter_callback) :
|
||||||
LR_MsgHandler(_f),
|
LR_MsgHandler(_f),
|
||||||
_set_parameter_callback(set_parameter_callback)
|
_set_parameter_callback(set_parameter_callback)
|
||||||
|
|
|
@ -7,6 +7,9 @@
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <sys/types.h>
|
#include <sys/types.h>
|
||||||
#include <sys/stat.h>
|
#include <sys/stat.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <signal.h>
|
||||||
|
|
||||||
|
|
||||||
#define DEBUG 1
|
#define DEBUG 1
|
||||||
#if DEBUG
|
#if DEBUG
|
||||||
|
@ -17,6 +20,8 @@
|
||||||
|
|
||||||
#define streq(x, y) (!strcmp(x, y))
|
#define streq(x, y) (!strcmp(x, y))
|
||||||
|
|
||||||
|
extern struct user_parameter *user_parameters;
|
||||||
|
|
||||||
LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
|
LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
|
||||||
AP_LoggerFileReader(),
|
AP_LoggerFileReader(),
|
||||||
_log_structure(log_structure),
|
_log_structure(log_structure),
|
||||||
|
@ -115,24 +120,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
||||||
if (in_list(name, log_write_names)) {
|
if (in_list(name, log_write_names)) {
|
||||||
// debug("%s is a Log_Write-written message\n", name);
|
// debug("%s is a Log_Write-written message\n", name);
|
||||||
} else {
|
} else {
|
||||||
if (in_list(name, generated_names)) {
|
if (!in_list(name, generated_names)) {
|
||||||
// debug("Log format for type (%d) (%s) taken from running code\n",
|
|
||||||
// f.type, name);
|
|
||||||
// bool found = false;
|
|
||||||
// for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
|
|
||||||
// if (streq(name, running_codes_log_structure[n].name)) {
|
|
||||||
// found = true;
|
|
||||||
// memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure));
|
|
||||||
// break;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// if (!found) {
|
|
||||||
// ::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name);
|
|
||||||
// abort();
|
|
||||||
// }
|
|
||||||
} else {
|
|
||||||
// debug("Log format for type (%d) (%s) taken from log\n", f.type, name);
|
|
||||||
// generate a LogStructure entry for this FMT
|
|
||||||
s.msg_type = map_fmt_type(name, f.type);
|
s.msg_type = map_fmt_type(name, f.type);
|
||||||
s.msg_len = f.length;
|
s.msg_len = f.length;
|
||||||
s.name = f.name;
|
s.name = f.name;
|
||||||
|
@ -150,7 +138,6 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
||||||
memcpy(pkt.name, s.name, sizeof(pkt.name));
|
memcpy(pkt.name, s.name, sizeof(pkt.name));
|
||||||
memcpy(pkt.format, s.format, sizeof(pkt.format));
|
memcpy(pkt.format, s.format, sizeof(pkt.format));
|
||||||
memcpy(pkt.labels, s.labels, sizeof(pkt.labels));
|
memcpy(pkt.labels, s.labels, sizeof(pkt.labels));
|
||||||
// AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (msgparser[f.type] != NULL) {
|
if (msgparser[f.type] != NULL) {
|
||||||
|
@ -161,7 +148,6 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
||||||
if (streq(name, "PARM")) {
|
if (streq(name, "PARM")) {
|
||||||
msgparser[f.type] = new LR_MsgHandler_PARM
|
msgparser[f.type] = new LR_MsgHandler_PARM
|
||||||
(formats[f.type],
|
(formats[f.type],
|
||||||
last_timestamp_usec,
|
|
||||||
[this](const char *xname, const float xvalue) {
|
[this](const char *xname, const float xvalue) {
|
||||||
return set_parameter(xname, xvalue);
|
return set_parameter(xname, xvalue);
|
||||||
});
|
});
|
||||||
|
@ -238,10 +224,6 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
|
||||||
// emit the output as we receive it:
|
// emit the output as we receive it:
|
||||||
AP::logger().WriteBlock(msg, f.length);
|
AP::logger().WriteBlock(msg, f.length);
|
||||||
|
|
||||||
// char name[5];
|
|
||||||
// memset(name, '\0', 5);
|
|
||||||
// memcpy(name, f.name, 4);
|
|
||||||
|
|
||||||
LR_MsgHandler *p = msgparser[f.type];
|
LR_MsgHandler *p = msgparser[f.type];
|
||||||
if (p == NULL) {
|
if (p == NULL) {
|
||||||
return true;
|
return true;
|
||||||
|
@ -252,11 +234,6 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <signal.h>
|
|
||||||
|
|
||||||
extern struct user_parameter *user_parameters;
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
see if a user parameter is set
|
see if a user parameter is set
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -10,24 +10,11 @@ class LogReader : public AP_LoggerFileReader
|
||||||
public:
|
public:
|
||||||
LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3);
|
LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3);
|
||||||
|
|
||||||
const Vector3f &get_attitude(void) const { return attitude; }
|
|
||||||
const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
|
|
||||||
const Vector3f &get_inavpos(void) const { return inavpos; }
|
|
||||||
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
|
|
||||||
const float &get_relalt(void) const { return rel_altitude; }
|
|
||||||
const LR_MsgHandler::CheckState &get_check_state(void) const { return check_state; }
|
|
||||||
|
|
||||||
VehicleType::vehicle_type vehicle;
|
VehicleType::vehicle_type vehicle;
|
||||||
|
|
||||||
bool check_user_param(const char *name);
|
bool check_user_param(const char *name);
|
||||||
bool set_parameter(const char *name, float value, bool force=false);
|
bool set_parameter(const char *name, float value, bool force=false);
|
||||||
|
|
||||||
void set_accel_mask(uint8_t mask) { accel_mask = mask; }
|
|
||||||
void set_gyro_mask(uint8_t mask) { gyro_mask = mask; }
|
|
||||||
void set_use_imt(bool _use_imt) { use_imt = _use_imt; }
|
|
||||||
void set_save_chek_messages(bool _save_chek_messages) { save_chek_messages = _save_chek_messages; }
|
|
||||||
|
|
||||||
uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
|
|
||||||
bool handle_log_format_msg(const struct log_Format &f) override;
|
bool handle_log_format_msg(const struct log_Format &f) override;
|
||||||
bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
|
bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
|
||||||
|
|
||||||
|
@ -43,35 +30,14 @@ private:
|
||||||
struct LogStructure *_log_structure;
|
struct LogStructure *_log_structure;
|
||||||
uint8_t _log_structure_count;
|
uint8_t _log_structure_count;
|
||||||
|
|
||||||
uint8_t accel_mask;
|
|
||||||
uint8_t gyro_mask;
|
|
||||||
bool use_imt = true;
|
|
||||||
|
|
||||||
uint32_t ground_alt_cm;
|
|
||||||
|
|
||||||
class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS] {};
|
class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS] {};
|
||||||
|
|
||||||
Vector3f attitude;
|
|
||||||
Vector3f ahr2_attitude;
|
|
||||||
Vector3f sim_attitude;
|
|
||||||
Vector3f inavpos;
|
|
||||||
float rel_altitude;
|
|
||||||
uint64_t last_timestamp_usec;
|
|
||||||
|
|
||||||
// mapping from original msgid to output msgid
|
// mapping from original msgid to output msgid
|
||||||
uint8_t mapped_msgid[256] {};
|
uint8_t mapped_msgid[256] {};
|
||||||
|
|
||||||
// next available msgid for mapping
|
// next available msgid for mapping
|
||||||
uint8_t next_msgid = 1;
|
uint8_t next_msgid = 1;
|
||||||
|
|
||||||
LR_MsgHandler::CheckState check_state;
|
|
||||||
|
|
||||||
bool installed_vehicle_specific_parsers;
|
|
||||||
|
|
||||||
bool save_chek_messages;
|
|
||||||
|
|
||||||
void maybe_install_vehicle_specific_parsers();
|
|
||||||
|
|
||||||
void initialise_fmt_map();
|
void initialise_fmt_map();
|
||||||
uint8_t map_fmt_type(const char *name, uint8_t intype);
|
uint8_t map_fmt_type(const char *name, uint8_t intype);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue