Merge pull request #191 from ISSUIUC/feature/gnc-ext-kf-telemetry. Done!

Extended KF telemetry

Author: MuhammadAli8209

MIDAS/src/gnc/ekf.cpp

@@ -82,7 +82,7 @@ void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceler
     sensor_accel_global_g(2, 0) = acceleration.az - 0.06f;
     euler_t angles_rad = orientation.getEuler();
     BodyToGlobal(angles_rad, sensor_accel_global_g);
-    // Do not apply gravity if on pad
+    // Do not apply acceleration if on pad
     float g_ms2 = (fsm > FSMState::STATE_IDLE) ? gravity_ms2 : 0.0f;
     u_control(0, 0) = sensor_accel_global_g(0, 0) * g_ms2;
     u_control(1, 0) = sensor_accel_global_g(1, 0) * g_ms2;

MIDAS/src/telemetry.cpp

@@ -126,9 +126,9 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) {
     packet.roll_rate = roll_rate_hz / MAX_ROLL_RATE_HZ * 0xFF;
 
     // KF data
-    packet.kf_px = inv_convert_range<int16_t>(kalman.position.px, MAX_KF_XPOSITION_M);
-
-    // Serial.println(kalman.position.px);
+    packet.kf_px = inv_convert_range<int16_t>(kalman.position.px, MAX_KF_VPOSITION_M); // We should eventually switch this to unsigned, once we fix baro.
+    packet.kf_py = inv_convert_range<int16_t>(kalman.position.py, MAX_KF_LPOSITION_M);
+    packet.kf_pz = inv_convert_range<int16_t>(kalman.position.pz, MAX_KF_LPOSITION_M);
 
     packet.kf_vx = inv_convert_range<int16_t>(kalman.velocity.vx, MAX_KF_XVELOCITY_MS);
 

MIDAS/src/telemetry_packet.h

@@ -5,7 +5,8 @@
 
 #define MAX_TELEM_VOLTAGE_V 6.0f
 #define MAX_TELEM_CONT_I 0.2f
-#define MAX_KF_XPOSITION_M 20000.0f
+#define MAX_KF_VPOSITION_M 100000.0f   // Max vertical position of KF data (m)
+#define MAX_KF_LPOSITION_M 100000.0f   // Max lateral position of KF data (m)
 #define MAX_ROLL_RATE_HZ 10.0f
 #define MAX_ABS_ACCEL_RANGE_G 64
 #define MAX_KF_XVELOCITY_MS 2000.0f
@@ -38,7 +39,9 @@ struct TelemetryPacket {
 
     uint8_t callsign_gpsfix_satcount; //3 bits gpsfix, 4 bits sat count, 1 bit is_sustainer_callsign
     uint16_t kf_vx; // 16 bit meters/second
-    uint16_t kf_px;  // 16 bit meters
+    uint16_t kf_px; // 16 bit meters
+    uint16_t kf_py; // 16 bit meters
+    uint16_t kf_pz; // 16 bit meters
 
     uint32_t pyro; // 7 bit continuity x 4 channels, 4 bit unused
 
@@ -68,4 +71,4 @@ struct TelemetryCommand {
         return verify == std::array<char, 3>{{'A','R','K'}};
         #endif
     }
-};
+};

ground/src/feather_duo/Output.cpp

@@ -38,7 +38,7 @@ void printJSONField(const char* name, const char* val, bool comma = true) {
 void printPacketJson(FullTelemetryData const& packet) {
     bool is_heartbeat = packet.FSM_State == static_cast<uint8_t>(-1);
     char buff[1024]{};
-    int len = sprintf(buff, R"({"type": "data", "value": {"barometer_altitude": %f, "latitude": %f, "longitude": %f, "altitude": %i, "highG_ax": %f, "highG_ay": %f, "highG_az": %f, "battery_voltage": %f, "cam_battery_voltage": %f, "FSM_State": %i, "tilt_angle": %f, "frequency": %f, "RSSI": %f, "sat_count": %i, "kf_velocity": %f, "kf_position": %f, "is_sustainer": %i, "roll_rate": %f, "c_valid": %u, "c_on": %u, "c_rec": %u, "vtx_on": %u, "vmux_stat": %u, "cam_ack": %u, "cmd_ack": %i, "gps_fixtype": %i, "pyro_a": %f, "pyro_b": %f, "pyro_c": %f, "pyro_d": %f}})",
+    int len = sprintf(buff, R"({"type": "data", "value": {"barometer_altitude": %f, "latitude": %f, "longitude": %f, "altitude": %i, "highG_ax": %f, "highG_ay": %f, "highG_az": %f, "battery_voltage": %f, "cam_battery_voltage": %f, "FSM_State": %i, "tilt_angle": %f, "frequency": %f, "RSSI": %f, "sat_count": %i, "kf_velocity": %f, "kf_positionX": %f, "kf_positionY": %f, "kf_positionZ": %f, "is_sustainer": %i, "roll_rate": %f, "c_valid": %u, "c_on": %u, "c_rec": %u, "vtx_on": %u, "vmux_stat": %u, "cam_ack": %u, "cmd_ack": %i, "gps_fixtype": %i, "pyro_a": %f, "pyro_b": %f, "pyro_c": %f, "pyro_d": %f}})",
 
     // Barometer
     packet.barometer_altitude,
@@ -63,6 +63,8 @@ void printPacketJson(FullTelemetryData const& packet) {
     packet.sat_count,
     packet.kf_vx,
     packet.kf_px,
+    packet.kf_py,
+    packet.kf_pz,
     packet.is_sustainer,
     packet.roll_rate_hz,
 

ground/src/feather_duo/Packet.cpp

@@ -60,7 +60,9 @@ FullTelemetryData DecodePacket(const TelemetryPacket& packet, float frequency) {
     data.pyros[3] = ((float) ((packet.pyro >> 24) & (0xFF)) / 255.) * MAX_TELEM_VOLTAGE_V;
 
     // kalman filter    
-    data.kf_px = convert_range<int16_t>(packet.kf_px, MAX_KF_XPOSITION_M);
+    data.kf_px = convert_range<int16_t>(packet.kf_px, MAX_KF_VPOSITION_M);
+    data.kf_py = convert_range<int16_t>(packet.kf_py, MAX_KF_LPOSITION_M);
+    data.kf_pz = convert_range<int16_t>(packet.kf_pz, MAX_KF_LPOSITION_M);
     data.kf_vx = convert_range<int16_t>(packet.kf_vx, MAX_KF_XVELOCITY_MS);
 
     // Camera state

ground/src/feather_duo/Packet.h

@@ -5,7 +5,8 @@ typedef uint32_t systime_t;
 
 #define MAX_TELEM_VOLTAGE_V 6.0f
 #define MAX_TELEM_CONT_I 0.2f
-#define MAX_KF_XPOSITION_M 20000.0f
+#define MAX_KF_VPOSITION_M 100000.0f
+#define MAX_KF_LPOSITION_M 100000.0f
 #define MAX_ROLL_RATE_HZ 10.0f
 #define MAX_ABS_ACCEL_RANGE_G 64
 #define MAX_KF_XVELOCITY_MS 2000.0f
@@ -38,16 +39,20 @@ struct TelemetryPacket {
 
     uint8_t callsign_gpsfix_satcount; //3 bits gpsfix, 4 bits sat count, 1 bit is_sustainer_callsign
     uint16_t kf_vx; // 16 bit meters/second
-    uint16_t kf_px;  // 16 bit meters
-    uint32_t pyro; // 7 bit continuity x 4 channels, 4 unused bits
+    uint16_t kf_px; // 16 bit meters
+    uint16_t kf_py; // 16 bit meters
+    uint16_t kf_pz; // 16 bit meters
+
+    uint32_t pyro; // 7 bit continuity x 4 channels, 4 bit unused
 
     uint8_t roll_rate;
     uint8_t camera_state;
     uint8_t camera_batt_volt;
-    
 };
 
 
+
+
 struct FullTelemetryData {
 
     systime_t timestamp;  //[0, 2^32]
@@ -78,6 +83,8 @@ struct FullTelemetryData {
 
     float kf_vx; // kalman filter stuff
     float kf_px;
+    float kf_py;
+    float kf_pz;
 
     bool cmd_ack;