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

Replay: added accel and gyro mask options

This commit is contained in:
Andrew Tridgell 2014-03-02 08:00:13 +11:00
parent 3218ac8e7a
commit ecdcf70ef5
3 changed files with 36 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Tools/Replay/LogReader.cpp
View File

@ -25,7 +25,9 @@ LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL
baro(_baro),
compass(_compass),
gps(_gps),
airspeed(_airspeed)
airspeed(_airspeed),
accel_mask(3),
gyro_mask(3)
{}
bool LogReader::open_log(const char *logfile)
@ -285,8 +287,12 @@ bool LogReader::update(uint8_t &type)
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
if (gyro_mask & 1) {
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 1) {
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
@ -298,8 +304,12 @@ bool LogReader::update(uint8_t &type)
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
if (gyro_mask & 2) {
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 2) {
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}

6
Tools/Replay/LogReader.h
View File

@ -31,6 +31,9 @@ public:
bool set_parameter(const char *name, float value);
void set_accel_mask(uint8_t mask) { accel_mask = mask; }
void set_gyro_mask(uint8_t mask) { gyro_mask = mask; }
private:
int fd;
AP_InertialSensor &ins;
@ -39,6 +42,9 @@ private:
GPS *&gps;
AP_Airspeed &airspeed;
uint8_t accel_mask;
uint8_t gyro_mask;
uint32_t ground_alt_cm;
uint8_t num_formats;

18
Tools/Replay/Replay.pde
View File

@ -98,6 +98,8 @@ static void usage(void)
::printf("Options:\n");
::printf(" -rRATE set IMU rate in Hz\n");
::printf(" -pNAME=VALUE set parameter NAME to VALUE\n");
::printf(" -aMASK set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n");
::printf(" -gMASK set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n");
}
void setup()
@ -111,15 +113,24 @@ void setup()
hal.util->commandline_arguments(argc, argv);
while ((opt = getopt(argc, argv, "r:p:h")) != -1) {
while ((opt = getopt(argc, argv, "r:p:ha:")) != -1) {
switch (opt) {
case 'h':
usage();
exit(0);
case 'r':
update_rate = strtol(optarg, NULL, 0);
break;
case 'g':
LogReader.set_gyro_mask(strtol(optarg, NULL, 0));
break;
case 'a':
LogReader.set_accel_mask(strtol(optarg, NULL, 0));
break;
case 'p':
char *eq = strchr(optarg, '=');
if (eq == NULL) {
@ -315,7 +326,8 @@ void loop()
Vector3f magVar;
float tasVar;
ahrs.get_secondary_attitude(DCM_attitude);
const Matrix3f &dcm_matrix = ((AP_AHRS_DCM)ahrs).get_dcm_matrix();
dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z);
NavEKF.getEulerAngles(ekf_euler);
NavEKF.getVelNED(velNED);
NavEKF.getPosNED(posNED);
@ -326,7 +338,7 @@ void loop()
NavEKF.getMagXYZ(magXYZ);
NavEKF.getInnovations(velInnov, posInnov, magInnov, tasInnov);
NavEKF.getVariances(velVar, posVar, magVar, tasVar);
ahrs.get_relative_position_NED(ekf_relpos);
NavEKF.getPosNED(ekf_relpos);
Vector3f inav_pos = inertial_nav.get_position() * 0.01f;
float temp = degrees(ekf_euler.z);