Skip to content

Commit

Permalink
New telemetry packet (#45)
Browse files Browse the repository at this point in the history 
* packet definition

* progress

* packet packing + new frequencies

* Update telemetry_packet.h

* Update fsm_states.h
  • Loading branch information
@NPIPHI
NPIPHI authored Apr 23, 2024
1 parent e10dc6e commit 6b5168e
Show file tree
Hide file tree
Showing 7 changed files with 45 additions and 211 deletions.
1 change: 1 addition & 0 deletions MIDAS/src/finite-state-machines/fsm_states.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,4 +19,5 @@ enum FSMState {
// #else
STATE_FIRST_SEPARATION,
// #endif
FSM_STATE_COUNT, //used to get the total number of fsm states so we can assert that the fsm will fit in 4 bits
};
9 changes: 6 additions & 3 deletions MIDAS/src/hardware/telemetry_backend.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@

// Change to 434.0 or other frequency, must match RX's freq!
#ifdef IS_BOOSTER
#define RF95_FREQ 433.0
#define RF95_FREQ 425.15
#else
#define RF95_FREQ 434.0
#define RF95_FREQ 426.15
#endif

TelemetryBackend::TelemetryBackend() : rf95(RFM96_CS, RFM96_INT) {
Expand All @@ -53,7 +53,10 @@ ErrorCode TelemetryBackend::init() {
if (!rf95.setFrequency(RF95_FREQ)) {
return ErrorCode::RadioSetFrequencyFailed;
}

rf95.setSignalBandwidth(125000);
rf95.setCodingRate4(8);
rf95.setSpreadingFactor(10);
rf95.setPayloadCRC(true);
/*
* The default transmitter power is 13dBm, using PA_BOOST.
* If you are using RFM95/96/97/98 modules which uses the PA_BOOST
Expand Down
1 change: 0 additions & 1 deletion MIDAS/src/rocket_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,5 @@ struct RocketData {
SensorData<Orientation> orientation;
SensorData<Voltage> voltage;

Latency telem_latency;
Latency log_latency;
};
14 changes: 0 additions & 14 deletions MIDAS/src/systems.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,25 +176,12 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
}
}


/**
* See \ref data_logger_thread
*/
DECLARE_THREAD(telemetry_buffering, RocketSystems* arg) {
while (true) {
arg->tlm.bufferData(arg->rocket_data);
THREAD_SLEEP(260/4);
}
}


/**
* See \ref data_logger_thread
*/
DECLARE_THREAD(telemetry, RocketSystems* arg) {
while (true) {
arg->tlm.transmit(arg->rocket_data, arg->led);
arg->rocket_data.telem_latency.tick();

THREAD_SLEEP(1);
}
Expand Down Expand Up @@ -257,7 +244,6 @@ ErrorCode init_systems(RocketSystems& systems) {
START_THREAD(fsm, SENSOR_CORE, config, 8);
START_THREAD(buzzer, SENSOR_CORE, config, 6);
START_THREAD(telemetry, SENSOR_CORE, config, 15);
START_THREAD(telemetry_buffering, SENSOR_CORE, config, 14);

config->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH);

Expand Down
152 changes: 29 additions & 123 deletions MIDAS/src/telemetry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,150 +20,56 @@ T inv_convert_range(float val, float range) {
return std::max(std::min((float)std::numeric_limits<T>::max(), converted), (float)std::numeric_limits<T>::min());
}

std::tuple<uint16_t, uint16_t, uint16_t> pack_highg_tilt(HighGData const& highg, uint8_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);

return {x,y,z};
}


Telemetry::Telemetry(TelemetryBackend&& backend) : backend(std::move(backend)) { }


void Telemetry::transmit(RocketData& rocket_data, LEDController& led) {
// telemetry_command command { };
// while (backend.read(&command)) {
// handleCommand(command);
// }

// if (!std::isnan(set_frequency_to)) {
// backend.setFrequency(set_frequency_to);
// set_frequency_to = NAN;
// }
static_assert(sizeof(TelemetryPacket) == 20);

TelemetryPacket packet = makePacket(rocket_data);
led.toggle(LED::BLUE);
backend.send(packet);
}


/**
* @brief This function handles commands sent from the ground station
* to TARS. The effects of this function depend on the command
* sent.
*
* @param cmd: struct containing information necessary to process
* ground station command.
*
* @return void
*/
void Telemetry::handleCommand(const telemetry_command &cmd) {
/* Check if the security code is present and matches on ground and on the rocket */
if (cmd.verify != std::array<char, 6>{'A', 'Y', 'B', 'E', 'R', 'K'}) {
return;
}
/* Check if lasted command ID matched current command ID */
if (last_command_id == cmd.cmd_id) {
return;
}
last_command_id = (int16_t)cmd.cmd_id;

if (cmd.command == SET_FREQ) {
set_frequency_to = cmd.freq;
}

if (cmd.command == SET_CALLSIGN) {
size_t callsign_size = sizeof(callsign);

for (size_t i = 0; i < callsign_size; ++i) {
callsign[i] = cmd.callsign[i];
}
Serial.println("[DEBUG]: Got callsign");
}
}


TelemetryPacket Telemetry::makePacket(RocketData& data) {
TelemetryPacket packet { };

TelemetryDataLite small_packet { };
packet.datapoint_count = 0;
for (int8_t i = 0; i < 4 && small_packet_queue.receive(&small_packet); i++) {
packet.datapoints[i] = small_packet;
packet.datapoint_count++;
}

GPS gps = data.gps.getRecentUnsync();
Magnetometer magnetometer = data.magnetometer.getRecentUnsync();
Voltage voltage = data.voltage.getRecentUnsync();
packet.voltage_battery = inv_convert_range<uint16_t>(voltage.voltage, 4096);
Barometer barometer = data.barometer.getRecentUnsync();
LowGLSM lowGlsm = data.low_g_lsm.getRecentUnsync();

packet.gps_lat = gps.latitude;
packet.gps_long = gps.longitude;
packet.gps_alt = gps.altitude;

packet.mag_x = inv_convert_range<int16_t>(magnetometer.mx, 8);
packet.mag_y = inv_convert_range<int16_t>(magnetometer.my, 8);
packet.mag_z = inv_convert_range<int16_t>(magnetometer.mz, 8);
packet.gyro_x = inv_convert_range<int16_t>(lowGlsm.gx, 8192);
packet.gyro_y = inv_convert_range<int16_t>(lowGlsm.gy, 8192);
packet.gyro_z = inv_convert_range<int16_t>(lowGlsm.gz, 8192);

packet.rssi = backend.getRecentRssi();
packet.FSM_state = (char) data.fsm_state.getRecentUnsync();

packet.barometer_temp = inv_convert_range<int16_t>(barometer.temperature, 256);

auto pyros = data.pyro.getRecentUnsync();
packet.pyros_bits = 0;
packet.pyros_bits |= pyros.channels[0].is_armed << 0;
packet.pyros_bits |= pyros.channels[1].is_armed << 1;
packet.pyros_bits |= pyros.channels[2].is_armed << 2;
packet.pyros_bits |= pyros.channels[3].is_armed << 3;
packet.pyros_bits |= pyros.channels[0].is_firing << 4;
packet.pyros_bits |= pyros.channels[1].is_firing << 5;
packet.pyros_bits |= pyros.channels[2].is_firing << 6;
packet.pyros_bits |= pyros.channels[3].is_firing << 7;

FSMState fsm = data.fsm_state.getRecentUnsync();
Continuity continuity = data.continuity.getRecentUnsync();
packet.sense_pyro = inv_convert_range<uint16_t>(continuity.sense_pyro, 4096);
for (int i = 0; i < 4; i++) {
packet.continuity[i] = inv_convert_range<int8_t>(continuity.pins[i], 20);
}

packet.telem_latency = inv_convert_range<int8_t>((float) data.telem_latency.getLatency(), 1024);
packet.log_latency = inv_convert_range<int8_t>((float) data.log_latency.getLatency(), 1024);

#ifdef IS_BOOSTER
packet.is_booster = true;
#else
packet.is_booster = false;
#endif

memcpy(&packet.callsign, &callsign, sizeof(callsign));
HighGData highg = data.high_g.getRecentUnsync();
PyroState pyro = data.pyro.getRecentUnsync();

packet.lat = gps.latitude;
packet.lon = gps.longitude;
packet.alt = uint16_t(gps.altitude);
packet.baro_alt = uint16_t(barometer.altitude);
auto [ax,ay,az] = pack_highg_tilt(highg, 33);
packet.highg_ax = ax;
packet.highg_ay = ay;
packet.highg_az = az;
packet.batt_volt = inv_convert_range<uint8_t>(voltage.voltage, 16);
static_assert(FSMState::FSM_STATE_COUNT < 16);
uint8_t sat_count = gps.satellite_count < 16 ? gps.satellite_count : 15;
packet.fsm_satcount = ((uint8_t)fsm) | (sat_count << 4);

return packet;
}

void Telemetry::bufferData(RocketData& rocket) {
TelemetryDataLite data { };
data.timestamp = pdTICKS_TO_MS(xTaskGetTickCount());
data.barometer_pressure = inv_convert_range<uint16_t>(rocket.barometer.getRecentUnsync().pressure , 4096);

HighGData highGData = rocket.high_g.getRecentUnsync();
data.highG_ax = inv_convert_range<int16_t>(highGData.ax, 256);
data.highG_ay = inv_convert_range<int16_t>(highGData.ay, 256);
data.highG_az = inv_convert_range<int16_t>(highGData.az, 256);

LowGLSM lowGlsm = rocket.low_g_lsm.getRecentUnsync();
data.lowg_ax = inv_convert_range<int16_t>(lowGlsm.ax, 8);
data.lowg_ay = inv_convert_range<int16_t>(lowGlsm.ay, 8);
data.lowg_az = inv_convert_range<int16_t>(lowGlsm.az, 8);

Orientation orient = rocket.orientation.getRecentUnsync();
data.bno_pitch = inv_convert_range<int16_t>(orient.pitch , 8);
data.bno_yaw = inv_convert_range<int16_t>(orient.yaw , 8);
data.bno_roll = inv_convert_range<int16_t>(orient.roll , 8);

small_packet_queue.send(data);
}

ErrorCode __attribute__((warn_unused_result)) Telemetry::init() {
return backend.init();
}
19 changes: 0 additions & 19 deletions MIDAS/src/telemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,27 +20,8 @@ class Telemetry {
ErrorCode __attribute__((warn_unused_result)) init();

void transmit(RocketData& rocket_data, LEDController& led);
void bufferData(RocketData& rocket);

private:
Queue<TelemetryDataLite, 4> small_packet_queue;

// Initializing command ID
int16_t last_command_id = -1;

float set_frequency_to = NAN;

#ifdef IS_SUSTAINER
char callsign[8] = "KD9ZMJ";
#endif

#ifdef IS_BOOSTER
char callsign[8] = "KD9ZPM";
#endif


TelemetryPacket makePacket(RocketData& data);
void handleCommand(const telemetry_command& cmd);

TelemetryBackend backend;
};
60 changes: 9 additions & 51 deletions MIDAS/src/telemetry_packet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,58 +3,16 @@
#include <array>
#include <cstdint>

struct TelemetryDataLite {
uint32_t timestamp; //[0, 2^32]

uint16_t barometer_pressure; //[0, 4096]
int16_t highG_ax; //[128, -128]
int16_t highG_ay; //[128, -128]
int16_t highG_az; //[128, -128]
int16_t lowg_ax; // [-4, 4]
int16_t lowg_ay; // [-4, 4]
int16_t lowg_az; // [-4, 4]
int16_t bno_roll; //[-4,4]
int16_t bno_pitch; //[-4,4]
int16_t bno_yaw; //[-4,4]
};

struct TelemetryPacket {
int8_t datapoint_count; //[0,4]
TelemetryDataLite datapoints[4];

int32_t gps_lat;
int32_t gps_long;
float gps_alt;
int16_t mag_x; // [-4, 4]
int16_t mag_y; // [-4, 4]
int16_t mag_z; // [-4, 4]
int16_t gyro_x; // [-4096, 4096]
int16_t gyro_y; // [-4096, 4096]
int16_t gyro_z; // [-4096, 4096]
int8_t rssi; // [-128, 128]
uint16_t voltage_battery; // [0, 16]
uint8_t FSM_state; // [0,256]
int16_t barometer_temp; // [-128, 128]
uint16_t sense_pyro; // [0, 16]
int8_t continuity[4]; // [-10, 10]
/** Pyros bit format:
* Lowest order bit is 0
* 0: pyros_armed[0]
* 1: pyros_armed[1]
* 2: pyros_armed[2]
* 3: pyros_armed[3]
* 4: pyros_firing[0]
* 5: pyros_firing[1]
* 6: pyros_firing[2]
* 7: pyros_firing[3]
*/
uint8_t pyros_bits;

uint8_t telem_latency; // [0, 1024]
uint8_t log_latency; // [0, 1024]

bool is_booster;
char callsign[8];
int32_t lat;
int32_t lon;
uint16_t alt;
uint16_t baro_alt;
uint16_t highg_ax; //14 bit signed ax [-16,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;
};

// Commands transmitted from ground station to rocket
Expand Down

0 comments on commit 6b5168e

Please sign in to comment.