Add more telem attributes (#65)

* Add more telem attributes

* Add faster telem rate

* Set coding rate to 8

* Add work

* fix tilt comment / code

* Add kf velocity

---------

Co-authored-by: Michael K <mkarpov2@illinois.edu>

Author: zyunlam

MIDAS/src/hardware/telemetry_backend.cpp

@@ -61,9 +61,9 @@ ErrorCode TelemetryBackend::init() {
     if (!rf95.setFrequency(RF95_FREQ)) {
         return ErrorCode::RadioSetFrequencyFailed;
     }
-    rf95.setSignalBandwidth(125000);
+    rf95.setSignalBandwidth(250000);
     rf95.setCodingRate4(8);
-    rf95.setSpreadingFactor(10);
+    rf95.setSpreadingFactor(8);
     rf95.setPayloadCRC(true);
     /*
      * The default transmitter power is 13dBm, using PA_BOOST.

MIDAS/src/telemetry.cpp

@@ -2,6 +2,10 @@
 
 #include "telemetry.h"
 
+inline long map(long x, long in_min, long in_max, long out_min, long out_max) {
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
 
 /**
  * @brief This function maps an input value onto within a particular range into a fixed point value of a certin binary
@@ -28,16 +32,17 @@ T inv_convert_range(float val, float range) {
  * 
  * @return tuple with packed data
 */
-std::tuple<uint16_t, uint16_t, uint16_t> pack_highg_tilt(HighGData const& highg, uint8_t tilt) {
+std::tuple<uint16_t, uint16_t, uint16_t, uint16_t> pack_highg_tilt(HighGData const& highg, uint16_t tilt) {
     uint16_t ax = (uint16_t)inv_convert_range<int16_t>(highg.ax, 32);
     uint16_t ay = (uint16_t)inv_convert_range<int16_t>(highg.ay, 32);
     uint16_t az = (uint16_t)inv_convert_range<int16_t>(highg.az, 32);
 
     uint16_t x = (ax & 0xfffc) | ((tilt >> 0) & 0x3);
     uint16_t y = (ay & 0xfffc) | ((tilt >> 2) & 0x3);
     uint16_t z = (az & 0xfffc) | ((tilt >> 4) & 0x3);
+    uint16_t q = (tilt >> 6) & 15;
 
-    return {x,y,z};
+    return {x,y,z,q};
 }
 
 /**
@@ -52,7 +57,7 @@ Telemetry::Telemetry(TelemetryBackend&& backend) : backend(std::move(backend)) {
  * @param led led state to transmit
 */
 void Telemetry::transmit(RocketData& rocket_data, LEDController& led) {
-    static_assert(sizeof(TelemetryPacket) == 20);
+    // static_assert(sizeof(TelemetryPacket) == 20);
 
     TelemetryPacket packet = makePacket(rocket_data);
     led.toggle(LED::BLUE);
@@ -83,20 +88,30 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) {
     HighGData highg = data.high_g.getRecentUnsync();
     PyroState pyro = data.pyro.getRecentUnsync();
     Orientation orientation = data.orientation.getRecentUnsync();
+    KalmanData kalman = data.kalman.getRecentUnsync();
 
     packet.lat = gps.latitude;
     packet.lon = gps.longitude;
     packet.alt = (((int16_t) gps.altitude) & 0xfffe) | (received_count & 0x0001);    // Convert range of value so that we can also account for negative altitudes
     packet.baro_alt = inv_convert_range<int16_t>(barometer.altitude, 1 << 17);
 
-    auto [ax,ay,az] = pack_highg_tilt(highg, map(static_cast<long>(orientation.tilt * 100),0, 314, 0, 63));
+    auto [ax,ay,az, tilt_extra] = pack_highg_tilt(highg, map(static_cast<long>(orientation.tilt * 100),0, 314, 0, 1023));
     packet.highg_ax = ax;
     packet.highg_ay = ay;
     packet.highg_az = az;
     packet.batt_volt = inv_convert_range<uint8_t>(voltage.voltage, 16);
+    
+    const float max_volts = 12;
+    packet.pyro |= ((((uint16_t) (continuity.pins[0] / max_volts * 127)) & 0x7F) << (0 * 7));
+    packet.pyro |= ((((uint16_t) (continuity.pins[1] / max_volts * 127)) & 0x7F) << (1 * 7));
+    packet.pyro |= ((((uint16_t) (continuity.pins[2] / max_volts * 127)) & 0x7F) << (2 * 7));
+    packet.pyro |= ((((uint16_t) (continuity.pins[3] / max_volts * 127)) & 0x7F) << (3 * 7));
+    packet.pyro |= tilt_extra << 28;
+
     static_assert(FSMState::FSM_STATE_COUNT < 16);
     uint8_t sat_count = gps.satellite_count < 8 ? gps.satellite_count : 7;
     packet.fsm_callsign_satcount = ((uint8_t)fsm) | (sat_count << 4);
+    packet.kf_vx = kalman.velocity.vx;
 
     #ifdef IS_SUSTAINER
     packet.fsm_callsign_satcount |= (1 << 7);

MIDAS/src/telemetry_packet.h

@@ -18,13 +18,14 @@ struct TelemetryPacket {
     uint16_t highg_az; //14 bit signed ax [-16,16) 2 bit tilt angle
     uint8_t batt_volt;
 
-    
     // If callsign bit (highest bit of fsm_callsign_satcount) is set, the callsign is KD9ZMJ
     //
     // If callsign bit (highest bit of fsm_callsign_satcount) is not set, the callsign is KD9ZPM
 
     uint8_t fsm_callsign_satcount; //4 bit fsm state, 1 bit is_sustainer_callsign, 3 bits sat count
-};
+    uint16_t kf_vx; // 16 bit meters/second
+    uint32_t pyro; // 7 bit continuity 4 bit tilt
+}  __attribute__((packed));
 
 
 // Commands transmitted from ground station to rocket

ground/src/feather/main.cpp

@@ -33,7 +33,7 @@
 #define VoltagePin 14
 // #define LED 13 // Blinks on receipt
 
-float RF95_FREQ = 420;
+float RF95_FREQ = 426.15;
 float SUSTAINER_FREQ = 426.15;
 float BOOSTER_FREQ = 425.15;
 float GROUND_FREQ = 420;
@@ -75,13 +75,20 @@ float convert_range(T val, float range) {
 struct TelemetryPacket {
     int32_t lat;
     int32_t lon;
-    int16_t alt;
-    int16_t baro_alt;
+    uint16_t alt; //15 bit meters, 1 bit command ack
+    uint16_t baro_alt;
     uint16_t highg_ax; //14 bit signed ax [-16,16) 2 bit tilt angle
-    uint16_t highg_ay;  //1bit sign 13 bit unsigned [0,16) 2 bit tilt angle
-    uint16_t highg_az;  //1bit sign 13 bit unsigned [0,16) 2 bit tilt angle
+    uint16_t highg_ay; //14 bit signed ax [-16,16) 2 bit tilt angle
+    uint16_t highg_az; //14 bit signed ax [-16,16) 2 bit tilt angle
     uint8_t batt_volt;
-    uint8_t fsm_satcount;
+    
+    // If callsign bit (highest bit of fsm_callsign_satcount) is set, the callsign is KD9ZMJ
+    //
+    // If callsign bit (highest bit of fsm_callsign_satcount) is not set, the callsign is KD9ZPM
+    
+    uint8_t fsm_callsign_satcount; //4 bit fsm state, 1 bit is_sustainer_callsign, 3 bits sat count
+    uint16_t kf_vx; // 16 bit meters/second
+    uint32_t pyro; // 7 bit continuity 4 bit tilt
     float RSSI = 0.0;
 };
 
@@ -100,7 +107,9 @@ struct FullTelemetryData {
     float freq;
     float rssi;
     float sat_count;
+    float pyros[4];
     bool is_sustainer;
+    uint16_t kf_vx
 };
 
 
@@ -166,20 +175,25 @@ void EnqueuePacket(const TelemetryPacket& packet, float frequency) {
     data.longitude = ConvertGPS(packet.lon);
     data.barometer_altitude = convert_range<int16_t>(packet.baro_alt, 1 << 17);
     int tilt = decodeLastTwoBits(packet.highg_ax, packet.highg_ay, packet.highg_az);
+    tilt |= (packet.pyro >> 28 & (0xF)) << 6;
     int16_t ax = packet.highg_ax & 0xfffc;
     int16_t ay = packet.highg_ay & 0xfffc;
     int16_t az = packet.highg_az & 0xfffc;
     data.highG_ax = convert_range<int16_t>(ax, 32);
     data.highG_ay = convert_range<int16_t>(ay, 32);
     data.highG_az = convert_range<int16_t>(az, 32);
-    data.tilt_angle = tilt; //convert_range(tilt, 180); // [-90, 90]
+    data.tilt_angle = tilt / 1023. * 180; // Returns tilt angle in range [0, 180]
     data.battery_voltage = convert_range(packet.batt_volt, 16);
-    data.sat_count = packet.fsm_satcount >> 4 & 0b0111;
-    data.is_sustainer = (packet.fsm_satcount >> 7);
-    data.FSM_State = packet.fsm_satcount & 0b1111;
+    data.sat_count = packet.fsm_callsign_satcount >> 4 & 0b0111;
+    data.is_sustainer = (packet.fsm_callsign_satcount >> 7);
+    data.FSM_State = packet.fsm_callsign_satcount & 0b1111;
+    data.pyros[0] = ((float) ((packet.pyro >> 0) & (0x7F)) / 127.) * 12.;
+    data.pyros[1] = ((float) ((packet.pyro >> 7) & (0x7F)) / 127.) * 12.;
+    data.pyros[2] = ((float) ((packet.pyro >> 14) & (0x7F)) / 127.) * 12.;
+    data.pyros[3] = ((float) ((packet.pyro >> 21) & (0x7F)) / 127.) * 12.;
 
     // kinda hacky but it will work
-    if (packet.fsm_satcount == static_cast<uint8_t>(-1)) {
+    if (packet.fsm_callsign_satcount == static_cast<uint8_t>(-1)) {
         data.FSM_State = static_cast<uint8_t>(-1);
     }
 
@@ -239,7 +253,12 @@ void printPacketJson(FullTelemetryData const& packet) {
     printJSONField("frequency", packet.freq);
     printJSONField("RSSI", packet.rssi);
     printJSONField("sat_count", packet.sat_count);
-    printJSONField("is_sustainer", packet.is_sustainer, false);
+    printJSONField("kf_velocity", packet.kf_vx);
+    printJSONField("is_sustainer", packet.is_sustainer);
+    printJSONField("pyro_a", packet.pyros[0]);
+    printJSONField("pyro_b", packet.pyros[1]);
+    printJSONField("pyro_c", packet.pyros[2]);
+    printJSONField("pyro_d", packet.pyros[3], false);
     Serial.println("}}");
 }
 
@@ -344,8 +363,10 @@ void setup() {
     rf95.setCodingRate4(8);
     rf95.setSpreadingFactor(8);
     rf95.setPayloadCRC(true);
-    rf95.setSignalBandwidth(125000);
+
+    rf95.setSignalBandwidth(250000);
     rf95.setPreambleLength(8);
+
     Serial.print(R"({"type": "freq_success", "frequency":)");
     Serial.print(current_freq);
     Serial.println("}");