Replay: merge MessageHandler_* into MsgHandler.h/MsgHandler.cpp

These were trivial classes; having lots of these files around made
it painful to work with.

Tools/Replay/LogReader.cpp

@@ -17,23 +17,6 @@
 #include <fcntl.h>
 
 #include "MsgHandler.h"
-#include "MsgHandler_PARM.h"
-#include "MsgHandler_GPS.h"
-#include "MsgHandler_GPS2.h"
-#include "MsgHandler_MSG.h"
-#include "MsgHandler_IMU.h"
-#include "MsgHandler_IMU2.h"
-#include "MsgHandler_IMU3.h"
-#include "MsgHandler_SIM.h"
-#include "MsgHandler_BARO.h"
-#include "MsgHandler_ARM.h"
-#include "MsgHandler_Event.h"
-#include "MsgHandler_AHR2.h"
-#include "MsgHandler_ATT.h"
-#include "MsgHandler_MAG.h"
-#include "MsgHandler_MAG2.h"
-#include "MsgHandler_NTUN_Copter.h"
-#include "MsgHandler_ARSP.h"
 
 #define streq(x, y) (!strcmp(x, y))
 

Tools/Replay/MsgHandler.cpp

@@ -287,3 +287,283 @@ void MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
     require_field(msg, "TimeMS", timestamp);
     wait_timestamp(timestamp);
 }
+
+
+
+/*
+ * subclasses to handle specific messages below here
+*/
+
+void MsgHandler_AHR2::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
+}
+
+
+void MsgHandler_ARM::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
+    hal.util->set_soft_armed(ArmState);
+    printf("Armed state: %u at %lu\n", 
+           (unsigned)ArmState,
+           (unsigned long)hal.scheduler->millis());
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+void MsgHandler_ARSP::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+
+    airspeed.setHIL(require_field_float(msg, "Airspeed"),
+		    require_field_float(msg, "DiffPress"),
+		    require_field_float(msg, "Temp"));
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+void MsgHandler_ATT::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
+}
+
+
+void MsgHandler_BARO::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    baro.setHIL(0,
+		require_field_float(msg, "Press"),
+		require_field_int16_t(msg, "Temp") * 0.01f);
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+#define DATA_ARMED                          10
+#define DATA_DISARMED                       11
+
+void MsgHandler_Event::process_message(uint8_t *msg)
+{
+    uint8_t id = require_field_uint8_t(msg, "Id");
+    if (id == DATA_ARMED) {
+        hal.util->set_soft_armed(true);
+        printf("Armed at %lu\n", 
+               (unsigned long)hal.scheduler->millis());
+    } else if (id == DATA_DISARMED) {
+        hal.util->set_soft_armed(false);
+        printf("Disarmed at %lu\n", 
+               (unsigned long)hal.scheduler->millis());
+    }
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+void MsgHandler_GPS2::process_message(uint8_t *msg)
+{
+    // only LOG_GPS_MSG gives us relative altitude.  We still log
+    // the relative altitude when we get a LOG_GPS2_MESSAGE - but
+    // the value we use (probably) comes from the most recent
+    // LOG_GPS_MESSAGE message!
+    update_from_msg_gps(1, msg, false);
+}
+
+
+void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
+{
+    uint32_t timestamp;
+    require_field(msg, "T", timestamp);
+    wait_timestamp(timestamp);
+
+    Location loc;
+    location_from_msg(msg, loc, "Lat", "Lng", "Alt");
+    Vector3f vel;
+    ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");
+
+    uint8_t status = require_field_uint8_t(msg, "Status");
+    gps.setHIL(gps_offset,
+               (AP_GPS::GPS_Status)status,
+               timestamp,
+               loc,
+               vel,
+               require_field_uint8_t(msg, "NSats"),
+               require_field_uint8_t(msg, "HDop"),
+               require_field_float(msg, "VZ") != 0);
+    if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
+        ground_alt_cm = require_field_int32_t(msg, "Alt");
+    }
+
+    if (responsible_for_relalt) {
+        // this could possibly check for the presence of "RelAlt" label?
+        rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt");
+    }
+
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+
+void MsgHandler_GPS::process_message(uint8_t *msg)
+{
+    update_from_msg_gps(0, msg, true);
+}
+
+
+void MsgHandler_IMU2::process_message(uint8_t *msg)
+{
+  update_from_msg_imu(1, msg);
+}
+
+
+void MsgHandler_IMU3::process_message(uint8_t *msg)
+{
+  update_from_msg_imu(2, msg);
+}
+
+
+void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+
+    uint8_t this_imu_mask = 1 << imu_offset;
+
+    if (gyro_mask & this_imu_mask) {
+        Vector3f gyro;
+        require_field(msg, "Gyr", gyro);
+        ins.set_gyro(imu_offset, gyro);
+    }
+    if (accel_mask & this_imu_mask) {
+        Vector3f accel2;
+        require_field(msg, "Acc", accel2);
+        ins.set_accel(imu_offset, accel2);
+    }
+
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+void MsgHandler_IMU::process_message(uint8_t *msg)
+{
+    update_from_msg_imu(0, msg);
+}
+
+
+void MsgHandler_MAG2::process_message(uint8_t *msg)
+{
+    update_from_msg_compass(1, msg);
+}
+
+
+void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+
+    Vector3f mag;
+    require_field(msg, "Mag", mag);
+    Vector3f mag_offset;
+    require_field(msg, "Ofs", mag_offset);
+
+    compass.setHIL(compass_offset, mag - mag_offset);
+    // compass_offset is which compass we are setting info for;
+    // mag_offset is a vector indicating the compass' calibration...
+    compass.set_offsets(compass_offset, mag_offset);
+
+    dataflash.WriteBlock(msg, f.length);
+}
+
+
+
+void MsgHandler_MAG::process_message(uint8_t *msg)
+{
+    update_from_msg_compass(0, msg);
+}
+
+#include <AP_AHRS.h>
+#include <VehicleType.h>
+
+void MsgHandler_MSG::process_message(uint8_t *msg)
+{
+    const uint8_t msg_text_len = 64;
+    char msg_text[msg_text_len];
+    require_field(msg, "Message", msg_text, msg_text_len);
+
+    if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
+	vehicle = VehicleType::VEHICLE_PLANE;
+	::printf("Detected Plane\n");
+	ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
+	ahrs.set_fly_forward(true);
+    } else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
+	       strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
+	vehicle = VehicleType::VEHICLE_COPTER;
+	::printf("Detected Copter\n");
+	ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
+	ahrs.set_fly_forward(false);
+    } else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
+	vehicle = VehicleType::VEHICLE_ROVER;
+	::printf("Detected Rover\n");
+	ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
+	ahrs.set_fly_forward(true);
+    }
+    dataflash.Log_Write_Message(msg_text);
+}
+
+
+void MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
+{
+    inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
+		       require_field_float(msg, "PosY") * 0.01f,
+		       0);
+}
+
+
+bool MsgHandler::set_parameter(const char *name, float value)
+{
+    const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" };
+    for (uint8_t i=0; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) {
+        if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
+            ::printf("Ignoring set of %s to %f\n", name, value);
+            return true;
+        }
+    }
+    enum ap_var_type var_type;
+    AP_Param *vp = AP_Param::find(name, &var_type);
+    if (vp == NULL) {
+        return false;
+    }
+    if (var_type == AP_PARAM_FLOAT) {
+        ((AP_Float *)vp)->set(value);
+        ::printf("Set %s to %f\n", name, value);
+    } else if (var_type == AP_PARAM_INT32) {
+        ((AP_Int32 *)vp)->set(value);
+        ::printf("Set %s to %d\n", name, (int)value);
+    } else if (var_type == AP_PARAM_INT16) {
+        ((AP_Int16 *)vp)->set(value);
+        ::printf("Set %s to %d\n", name, (int)value);
+    } else if (var_type == AP_PARAM_INT8) {
+        ((AP_Int8 *)vp)->set(value);
+        ::printf("Set %s to %d\n", name, (int)value);
+    } else {
+        // we don't support mavlink set on this parameter
+        return false;
+    }
+    return true;
+}
+
+void MsgHandler_PARM::process_message(uint8_t *msg)
+{
+    const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
+    char parameter_name[parameter_name_len];
+
+    require_field(msg, "Name", parameter_name, parameter_name_len);
+
+    set_parameter(parameter_name, require_field_float(msg, "Value"));
+}
+
+
+void MsgHandler_SIM::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+    attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");
+}
+

Tools/Replay/MsgHandler.h

@@ -2,6 +2,7 @@
 #define AP_MSGHANDLER_H
 
 #include <DataFlash.h>
+#include <VehicleType.h>
 
 #include <stdio.h>
 
@@ -158,4 +159,303 @@ inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg,
     }
 }
 
+
+/* subclasses below this point */
+
+class MsgHandler_AHR2 : public MsgHandler
+{
+public:
+    MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
+        : MsgHandler(_f, _dataflash,_last_timestamp_usec),
+          ahr2_attitude(_ahr2_attitude) { };
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    Vector3f &ahr2_attitude;
+};
+
+
+class MsgHandler_ARM : public MsgHandler
+{
+public:
+    MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec)
+        : MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
+
+    virtual void process_message(uint8_t *msg);
+};
+
+
+class MsgHandler_ARSP : public MsgHandler
+{
+public:
+    MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
+		    uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
+	MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    AP_Airspeed &airspeed;
+};
+
+
+class MsgHandler_ATT : public MsgHandler
+{
+public:
+    MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec, Vector3f &_attitude)
+        : MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
+        { };
+    virtual void process_message(uint8_t *msg);
+
+private:
+    Vector3f &attitude;
+};
+
+
+class MsgHandler_BARO : public MsgHandler
+{
+public:
+    MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec, AP_Baro &_baro)
+        : MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { };
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    AP_Baro &baro;
+};
+
+
+class MsgHandler_Event : public MsgHandler
+{
+public:
+    MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec)
+        : MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
+
+    virtual void process_message(uint8_t *msg);
+};
+
+
+
+
+class MsgHandler_GPS_Base : public MsgHandler
+{
+
+public:
+    MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
+                        uint64_t &_last_timestamp_usec, AP_GPS &_gps,
+                        uint32_t &_ground_alt_cm, float &_rel_altitude)
+        : MsgHandler(_f, _dataflash, _last_timestamp_usec),
+          gps(_gps), ground_alt_cm(_ground_alt_cm),
+          rel_altitude(_rel_altitude) { };
+
+protected:
+    void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt);
+
+private:
+    AP_GPS &gps;
+    uint32_t &ground_alt_cm;
+    float &rel_altitude;
+};
+
+
+class MsgHandler_GPS : public MsgHandler_GPS_Base
+{
+public:
+    MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec, AP_GPS &_gps,
+                   uint32_t &_ground_alt_cm, float &_rel_altitude)
+        : MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
+                              _gps, _ground_alt_cm, _rel_altitude),
+          gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
+
+    void process_message(uint8_t *msg);
+
+private:
+    AP_GPS &gps;
+    uint32_t &ground_alt_cm;
+    float &rel_altitude;
+};
+
+// it would be nice to use the same parser for both GPS message types
+// (and other packets, too...).  I*think* the contructor can simply
+// take e.g. &gps[1]... problems are going to arise if we don't
+// actually have that many gps' compiled in!
+class MsgHandler_GPS2 : public MsgHandler_GPS_Base
+{
+public:
+    MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec, AP_GPS &_gps,
+                    uint32_t &_ground_alt_cm, float &_rel_altitude)
+        : MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
+                              _gps, _ground_alt_cm,
+                              _rel_altitude), gps(_gps),
+          ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
+    virtual void process_message(uint8_t *msg);
+private:
+    AP_GPS &gps;
+    uint32_t &ground_alt_cm;
+    float &rel_altitude;
+};
+
+
+
+class MsgHandler_IMU_Base : public MsgHandler
+{
+public:
+    MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
+                        uint64_t &_last_timestamp_usec,
+                        uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                        AP_InertialSensor &_ins) :
+        MsgHandler(_f, _dataflash, _last_timestamp_usec),
+        accel_mask(_accel_mask),
+        gyro_mask(_gyro_mask),
+        ins(_ins) { };
+    void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg);
+
+private:
+    uint8_t &accel_mask;
+    uint8_t &gyro_mask;
+    AP_InertialSensor &ins;
+};
+
+class MsgHandler_IMU : public MsgHandler_IMU_Base
+{
+public:
+    MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec,
+                   uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                   AP_InertialSensor &_ins)
+        : MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
+                              _accel_mask, _gyro_mask, _ins) { };
+
+    void process_message(uint8_t *msg);
+};
+
+class MsgHandler_IMU2 : public MsgHandler_IMU_Base
+{
+public:
+    MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec,
+                    uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                    AP_InertialSensor &_ins)
+        : MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
+                              _accel_mask, _gyro_mask, _ins) {};
+
+    virtual void process_message(uint8_t *msg);
+};
+
+class MsgHandler_IMU3 : public MsgHandler_IMU_Base
+{
+public:
+    MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec,
+                    uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                    AP_InertialSensor &_ins)
+        : MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
+                              _accel_mask, _gyro_mask, _ins) {};
+
+    virtual void process_message(uint8_t *msg);
+};
+
+
+
+class MsgHandler_MAG_Base : public MsgHandler
+{
+public:
+    MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
+                        uint64_t &_last_timestamp_usec, Compass &_compass)
+	: MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
+
+protected:
+    void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
+
+private:
+    Compass &compass;
+};
+
+class MsgHandler_MAG : public MsgHandler_MAG_Base
+{
+public:
+    MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec, Compass &_compass)
+        : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
+
+    virtual void process_message(uint8_t *msg);
+};
+
+class MsgHandler_MAG2 : public MsgHandler_MAG_Base
+{
+public:
+    MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
+                    uint64_t &_last_timestamp_usec, Compass &_compass)
+        : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
+
+    virtual void process_message(uint8_t *msg);
+};
+
+
+
+class MsgHandler_MSG : public MsgHandler
+{
+public:
+    MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec,
+                   VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
+        MsgHandler(_f, _dataflash, _last_timestamp_usec),
+        vehicle(_vehicle), ahrs(_ahrs) { }
+
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    VehicleType::vehicle_type &vehicle;
+    AP_AHRS &ahrs;
+};
+
+
+class MsgHandler_NTUN_Copter : public MsgHandler
+{
+public:
+    MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
+			   uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
+	: MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    Vector3f &inavpos;
+};
+
+
+class MsgHandler_PARM : public MsgHandler
+{
+public:
+    MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t _last_timestamp_usec) : MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
+
+    virtual void process_message(uint8_t *msg);
+};
+
+
+class MsgHandler_SIM : public MsgHandler
+{
+public:
+    MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec,
+                   Vector3f &_sim_attitude)
+        : MsgHandler(_f, _dataflash, _last_timestamp_usec),
+          sim_attitude(_sim_attitude)
+        { };
+
+    virtual void process_message(uint8_t *msg);
+
+private:
+    Vector3f &sim_attitude;
+};
+
 #endif

Tools/Replay/MsgHandler_AHR2.cpp

@@ -1,7 +0,0 @@
-#include "MsgHandler_AHR2.h"
-
-void MsgHandler_AHR2::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
-}

Tools/Replay/MsgHandler_AHR2.h

@@ -1,15 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_AHR2 : public MsgHandler
-{
-public:
-    MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
-                    uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
-        : MsgHandler(_f, _dataflash,_last_timestamp_usec),
-          ahr2_attitude(_ahr2_attitude) { };
-
-    virtual void process_message(uint8_t *msg);
-
-private:
-    Vector3f &ahr2_attitude;
-};

Tools/Replay/MsgHandler_ARM.cpp

@@ -1,14 +0,0 @@
-#include "MsgHandler_ARM.h"
-
-extern const AP_HAL::HAL& hal;
-
-void MsgHandler_ARM::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
-    hal.util->set_soft_armed(ArmState);
-    printf("Armed state: %u at %lu\n", 
-           (unsigned)ArmState,
-           (unsigned long)hal.scheduler->millis());
-    dataflash.WriteBlock(msg, f.length);
-}

Tools/Replay/MsgHandler_ARM.h

@@ -1,11 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_ARM : public MsgHandler
-{
-public:
-    MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec)
-        : MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_ARSP.cpp

@@ -1,11 +0,0 @@
-#include "MsgHandler_ARSP.h"
-
-void MsgHandler_ARSP::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    airspeed.setHIL(require_field_float(msg, "Airspeed"),
-		    require_field_float(msg, "DiffPress"),
-		    require_field_float(msg, "Temp"));
-    dataflash.WriteBlock(msg, f.length);
-}

Tools/Replay/MsgHandler_ARSP.h

@@ -1,14 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_ARSP : public MsgHandler
-{
-public:
-    MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
-		    uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
-	MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
-
-    virtual void process_message(uint8_t *msg);
-
-private:
-    AP_Airspeed &airspeed;
-};

Tools/Replay/MsgHandler_ATT.cpp

@@ -1,7 +0,0 @@
-#include "MsgHandler_ATT.h"
-
-void MsgHandler_ATT::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
-}

Tools/Replay/MsgHandler_ATT.h

@@ -1,14 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_ATT : public MsgHandler
-{
-public:
-    MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec, Vector3f &_attitude)
-        : MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
-        { };
-    virtual void process_message(uint8_t *msg);
-
-private:
-    Vector3f &attitude;
-};

Tools/Replay/MsgHandler_BARO.cpp

@@ -1,10 +0,0 @@
-#include "MsgHandler_BARO.h"
-
-void MsgHandler_BARO::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    baro.setHIL(0,
-		require_field_float(msg, "Press"),
-		require_field_int16_t(msg, "Temp") * 0.01f);
-    dataflash.WriteBlock(msg, f.length);
-}

Tools/Replay/MsgHandler_BARO.h

@@ -1,14 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_BARO : public MsgHandler
-{
-public:
-    MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
-                    uint64_t &_last_timestamp_usec, AP_Baro &_baro)
-        : MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { };
-
-    virtual void process_message(uint8_t *msg);
-
-private:
-    AP_Baro &baro;
-};

Tools/Replay/MsgHandler_Event.cpp

@@ -1,21 +0,0 @@
-#include "MsgHandler_Event.h"
-
-extern const AP_HAL::HAL& hal;
-
-#define DATA_ARMED                          10
-#define DATA_DISARMED                       11
-
-void MsgHandler_Event::process_message(uint8_t *msg)
-{
-    uint8_t id = require_field_uint8_t(msg, "Id");
-    if (id == DATA_ARMED) {
-        hal.util->set_soft_armed(true);
-        printf("Armed at %lu\n", 
-               (unsigned long)hal.scheduler->millis());
-    } else if (id == DATA_DISARMED) {
-        hal.util->set_soft_armed(false);
-        printf("Disarmed at %lu\n", 
-               (unsigned long)hal.scheduler->millis());
-    }
-    dataflash.WriteBlock(msg, f.length);
-}

Tools/Replay/MsgHandler_Event.h

@@ -1,11 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_Event : public MsgHandler
-{
-public:
-    MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec)
-        : MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_GPS.cpp

@@ -1,6 +0,0 @@
-#include "MsgHandler_GPS.h"
-
-void MsgHandler_GPS::process_message(uint8_t *msg)
-{
-    update_from_msg_gps(0, msg, true);
-}

Tools/Replay/MsgHandler_GPS.h

@@ -1,19 +0,0 @@
-#include "MsgHandler_GPS_Base.h"
-
-class MsgHandler_GPS : public MsgHandler_GPS_Base
-{
-public:
-    MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                   uint32_t &_ground_alt_cm, float &_rel_altitude)
-        : MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
-                              _gps, _ground_alt_cm, _rel_altitude),
-          gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
-
-    void process_message(uint8_t *msg);
-
-private:
-    AP_GPS &gps;
-    uint32_t &ground_alt_cm;
-    float &rel_altitude;
-};

Tools/Replay/MsgHandler_GPS2.cpp

@@ -1,10 +0,0 @@
-#include <MsgHandler_GPS2.h>
-
-void MsgHandler_GPS2::process_message(uint8_t *msg)
-{
-    // only LOG_GPS_MSG gives us relative altitude.  We still log
-    // the relative altitude when we get a LOG_GPS2_MESSAGE - but
-    // the value we use (probably) comes from the most recent
-    // LOG_GPS_MESSAGE message!
-    update_from_msg_gps(1, msg, false);
-}

Tools/Replay/MsgHandler_GPS2.h

@@ -1,22 +0,0 @@
-#include "MsgHandler_GPS_Base.h"
-
-// it would be nice to use the same parser for both GPS message types
-// (and other packets, too...).  I*think* the contructor can simply
-// take e.g. &gps[1]... problems are going to arise if we don't
-// actually have that many gps' compiled in!
-class MsgHandler_GPS2 : public MsgHandler_GPS_Base
-{
-public:
-    MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
-                    uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                    uint32_t &_ground_alt_cm, float &_rel_altitude)
-        : MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
-                              _gps, _ground_alt_cm,
-                              _rel_altitude), gps(_gps),
-          ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
-    virtual void process_message(uint8_t *msg);
-private:
-    AP_GPS &gps;
-    uint32_t &ground_alt_cm;
-    float &rel_altitude;
-};

Tools/Replay/MsgHandler_GPS_Base.cpp

@@ -1,34 +0,0 @@
-#include "MsgHandler_GPS_Base.h"
-
-void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
-{
-    uint32_t timestamp;
-    require_field(msg, "T", timestamp);
-    wait_timestamp(timestamp);
-
-    Location loc;
-    location_from_msg(msg, loc, "Lat", "Lng", "Alt");
-    Vector3f vel;
-    ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");
-
-    uint8_t status = require_field_uint8_t(msg, "Status");
-    gps.setHIL(gps_offset,
-               (AP_GPS::GPS_Status)status,
-               timestamp,
-               loc,
-               vel,
-               require_field_uint8_t(msg, "NSats"),
-               require_field_uint8_t(msg, "HDop"),
-               require_field_float(msg, "VZ") != 0);
-    if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
-        ground_alt_cm = require_field_int32_t(msg, "Alt");
-    }
-
-    if (responsible_for_relalt) {
-        // this could possibly check for the presence of "RelAlt" label?
-        rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt");
-    }
-
-    dataflash.WriteBlock(msg, f.length);
-}
-

Tools/Replay/MsgHandler_GPS_Base.h

@@ -1,27 +0,0 @@
-#include <MsgHandler.h>
-
-#ifndef AP_MSGHANDLER_GPS_BASE_H
-#define AP_MSGHANDLER_GPS_BASE_H
-
-class MsgHandler_GPS_Base : public MsgHandler
-{
-
-public:
-    MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
-                        uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                        uint32_t &_ground_alt_cm, float &_rel_altitude)
-        : MsgHandler(_f, _dataflash, _last_timestamp_usec),
-          gps(_gps), ground_alt_cm(_ground_alt_cm),
-          rel_altitude(_rel_altitude) { };
-
-protected:
-    void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt);
-
-private:
-    AP_GPS &gps;
-    uint32_t &ground_alt_cm;
-    float &rel_altitude;
-};
-
-#endif
-

Tools/Replay/MsgHandler_IMU.cpp

@@ -1,6 +0,0 @@
-#include "MsgHandler_IMU.h"
-
-void MsgHandler_IMU::process_message(uint8_t *msg)
-{
-    update_from_msg_imu(0, msg);
-}

Tools/Replay/MsgHandler_IMU.h

@@ -1,14 +0,0 @@
-#include "MsgHandler_IMU_Base.h"
-
-class MsgHandler_IMU : public MsgHandler_IMU_Base
-{
-public:
-    MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec,
-                   uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                   AP_InertialSensor &_ins)
-        : MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
-                              _accel_mask, _gyro_mask, _ins) { };
-
-    void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_IMU2.cpp

@@ -1,6 +0,0 @@
-#include "MsgHandler_IMU2.h"
-
-void MsgHandler_IMU2::process_message(uint8_t *msg)
-{
-  update_from_msg_imu(1, msg);
-}

Tools/Replay/MsgHandler_IMU2.h

@@ -1,14 +0,0 @@
-#include "MsgHandler_IMU_Base.h"
-
-class MsgHandler_IMU2 : public MsgHandler_IMU_Base
-{
-public:
-    MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
-                    uint64_t &_last_timestamp_usec,
-                    uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                    AP_InertialSensor &_ins)
-        : MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
-                              _accel_mask, _gyro_mask, _ins) {};
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_IMU3.cpp

@@ -1,6 +0,0 @@
-#include "MsgHandler_IMU3.h"
-
-void MsgHandler_IMU3::process_message(uint8_t *msg)
-{
-  update_from_msg_imu(2, msg);
-}

Tools/Replay/MsgHandler_IMU3.h

@@ -1,14 +0,0 @@
-#include "MsgHandler_IMU_Base.h"
-
-class MsgHandler_IMU3 : public MsgHandler_IMU_Base
-{
-public:
-    MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
-                    uint64_t &_last_timestamp_usec,
-                    uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                    AP_InertialSensor &_ins)
-        : MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
-                              _accel_mask, _gyro_mask, _ins) {};
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_IMU_Base.cpp

@@ -1,21 +0,0 @@
-#include "MsgHandler_IMU_Base.h"
-
-void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    uint8_t this_imu_mask = 1 << imu_offset;
-
-    if (gyro_mask & this_imu_mask) {
-        Vector3f gyro;
-        require_field(msg, "Gyr", gyro);
-        ins.set_gyro(imu_offset, gyro);
-    }
-    if (accel_mask & this_imu_mask) {
-        Vector3f accel2;
-        require_field(msg, "Acc", accel2);
-        ins.set_accel(imu_offset, accel2);
-    }
-
-    dataflash.WriteBlock(msg, f.length);
-}

Tools/Replay/MsgHandler_IMU_Base.h

@@ -1,26 +0,0 @@
-#ifndef AP_MSGHANDLER_IMU_BASE_H
-#define AP_MSGHANDLER_IMU_BASE_H
-
-#include <MsgHandler.h>
-
-class MsgHandler_IMU_Base : public MsgHandler
-{
-public:
-    MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
-                        uint64_t &_last_timestamp_usec,
-                        uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                        AP_InertialSensor &_ins) :
-        MsgHandler(_f, _dataflash, _last_timestamp_usec),
-        accel_mask(_accel_mask),
-        gyro_mask(_gyro_mask),
-        ins(_ins) { };
-    void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg);
-
-private:
-    uint8_t &accel_mask;
-    uint8_t &gyro_mask;
-    AP_InertialSensor &ins;
-};
-
-#endif
-

Tools/Replay/MsgHandler_MAG.cpp

@@ -1,6 +0,0 @@
-#include "MsgHandler_MAG.h"
-
-void MsgHandler_MAG::process_message(uint8_t *msg)
-{
-    update_from_msg_compass(0, msg);
-}

Tools/Replay/MsgHandler_MAG.h

@@ -1,11 +0,0 @@
-#include "MsgHandler_MAG_Base.h"
-
-class MsgHandler_MAG : public MsgHandler_MAG_Base
-{
-public:
-    MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec, Compass &_compass)
-        : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_MAG2.cpp

@@ -1,6 +0,0 @@
-#include "MsgHandler_MAG2.h"
-
-void MsgHandler_MAG2::process_message(uint8_t *msg)
-{
-    update_from_msg_compass(1, msg);
-}

Tools/Replay/MsgHandler_MAG2.h

@@ -1,11 +0,0 @@
-#include "MsgHandler_MAG_Base.h"
-
-class MsgHandler_MAG2 : public MsgHandler_MAG_Base
-{
-public:
-    MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
-                    uint64_t &_last_timestamp_usec, Compass &_compass)
-        : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_MAG_Base.cpp

@@ -1,19 +0,0 @@
-#include "MsgHandler_MAG_Base.h"
-
-void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    Vector3f mag;
-    require_field(msg, "Mag", mag);
-    Vector3f mag_offset;
-    require_field(msg, "Ofs", mag_offset);
-
-    compass.setHIL(compass_offset, mag - mag_offset);
-    // compass_offset is which compass we are setting info for;
-    // mag_offset is a vector indicating the compass' calibration...
-    compass.set_offsets(compass_offset, mag_offset);
-
-    dataflash.WriteBlock(msg, f.length);
-}
-

Tools/Replay/MsgHandler_MAG_Base.h

@@ -1,20 +0,0 @@
-#ifndef MSGHANDLER_MAG_BASE_H
-#define MSGHANDLER_MAG_BASE_H
-
-#include "MsgHandler.h"
-
-class MsgHandler_MAG_Base : public MsgHandler
-{
-public:
-    MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
-                        uint64_t &_last_timestamp_usec, Compass &_compass)
-	: MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
-
-protected:
-    void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
-
-private:
-    Compass &compass;
-};
-
-#endif // MSGHANDLER_MAG_BASE

Tools/Replay/MsgHandler_MSG.cpp

@@ -1,29 +0,0 @@
-#include "MsgHandler_MSG.h"
-#include <AP_AHRS.h>
-#include <VehicleType.h>
-
-void MsgHandler_MSG::process_message(uint8_t *msg)
-{
-    const uint8_t msg_text_len = 64;
-    char msg_text[msg_text_len];
-    require_field(msg, "Message", msg_text, msg_text_len);
-
-    if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
-	vehicle = VehicleType::VEHICLE_PLANE;
-	::printf("Detected Plane\n");
-	ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
-	ahrs.set_fly_forward(true);
-    } else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
-	       strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
-	vehicle = VehicleType::VEHICLE_COPTER;
-	::printf("Detected Copter\n");
-	ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
-	ahrs.set_fly_forward(false);
-    } else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
-	vehicle = VehicleType::VEHICLE_ROVER;
-	::printf("Detected Rover\n");
-	ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
-	ahrs.set_fly_forward(true);
-    }
-    dataflash.Log_Write_Message(msg_text);
-}

Tools/Replay/MsgHandler_MSG.h

@@ -1,19 +0,0 @@
-#include <MsgHandler.h>
-#include <VehicleType.h>
-
-class MsgHandler_MSG : public MsgHandler
-{
-public:
-    MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec,
-                   VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
-        MsgHandler(_f, _dataflash, _last_timestamp_usec),
-        vehicle(_vehicle), ahrs(_ahrs) { }
-
-
-    virtual void process_message(uint8_t *msg);
-
-private:
-    VehicleType::vehicle_type &vehicle;
-    AP_AHRS &ahrs;
-};

Tools/Replay/MsgHandler_NTUN_Copter.cpp

@@ -1,8 +0,0 @@
-#include "MsgHandler_NTUN_Copter.h"
-
-void MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
-{
-    inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
-		       require_field_float(msg, "PosY") * 0.01f,
-		       0);
-}

Tools/Replay/MsgHandler_NTUN_Copter.h

@@ -1,14 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_NTUN_Copter : public MsgHandler
-{
-public:
-    MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
-			   uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
-	: MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
-
-    virtual void process_message(uint8_t *msg);
-
-private:
-    Vector3f &inavpos;
-};

Tools/Replay/MsgHandler_PARM.cpp

@@ -1,44 +0,0 @@
-#include "MsgHandler_PARM.h"
-
-bool MsgHandler::set_parameter(const char *name, float value)
-{
-    const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" };
-    for (uint8_t i=0; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) {
-        if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
-            ::printf("Ignoring set of %s to %f\n", name, value);
-            return true;
-        }
-    }
-    enum ap_var_type var_type;
-    AP_Param *vp = AP_Param::find(name, &var_type);
-    if (vp == NULL) {
-        return false;
-    }
-    if (var_type == AP_PARAM_FLOAT) {
-        ((AP_Float *)vp)->set(value);
-        ::printf("Set %s to %f\n", name, value);
-    } else if (var_type == AP_PARAM_INT32) {
-        ((AP_Int32 *)vp)->set(value);
-        ::printf("Set %s to %d\n", name, (int)value);
-    } else if (var_type == AP_PARAM_INT16) {
-        ((AP_Int16 *)vp)->set(value);
-        ::printf("Set %s to %d\n", name, (int)value);
-    } else if (var_type == AP_PARAM_INT8) {
-        ((AP_Int8 *)vp)->set(value);
-        ::printf("Set %s to %d\n", name, (int)value);
-    } else {
-        // we don't support mavlink set on this parameter
-        return false;
-    }
-    return true;
-}
-
-void MsgHandler_PARM::process_message(uint8_t *msg)
-{
-    const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
-    char parameter_name[parameter_name_len];
-
-    require_field(msg, "Name", parameter_name, parameter_name_len);
-
-    set_parameter(parameter_name, require_field_float(msg, "Value"));
-}

Tools/Replay/MsgHandler_PARM.h

@@ -1,9 +0,0 @@
-#include <MsgHandler.h>
-
-class MsgHandler_PARM : public MsgHandler
-{
-public:
-    MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t _last_timestamp_usec) : MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
-
-    virtual void process_message(uint8_t *msg);
-};

Tools/Replay/MsgHandler_SIM.cpp

@@ -1,7 +0,0 @@
-#include "MsgHandler_SIM.h"
-
-void MsgHandler_SIM::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");
-}

Tools/Replay/MsgHandler_SIM.h

@@ -1,17 +0,0 @@
-#include "MsgHandler.h"
-
-class MsgHandler_SIM : public MsgHandler
-{
-public:
-    MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
-                   uint64_t &_last_timestamp_usec,
-                   Vector3f &_sim_attitude)
-        : MsgHandler(_f, _dataflash, _last_timestamp_usec),
-          sim_attitude(_sim_attitude)
-        { };
-
-    virtual void process_message(uint8_t *msg);
-
-private:
-    Vector3f &sim_attitude;
-};