From fce0f1d6046f2691ed3c28bc70c3b1567ca4d723 Mon Sep 17 00:00:00 2001 From: Neelay Junnarkar Date: Thu, 25 Apr 2019 01:18:40 -0700 Subject: [PATCH 1/6] beginning refactor --- gs/gs.h | 739 ++++++++++++++++++-------------------------------------- 1 file changed, 237 insertions(+), 502 deletions(-) diff --git a/gs/gs.h b/gs/gs.h index ca8b015..99f622c 100644 --- a/gs/gs.h +++ b/gs/gs.h @@ -1,502 +1,237 @@ -/* - * CalSTAR Avionics Ground Station - * - * file: gs.h - * - * Provides communication between laptop and rocket. - * Communicates with laptop via UART over USB (with UART with an FTDI cable as - * backup) Communicates with rocket via radio, 433 MHz Has 2 configurable LEDs - * and 4 digital inputs on terminal blocks - * - * LEDS: - * - Rx (PB12) - * - Tx (PB13) - * - * Digital Inputs: - * - IO 1 (PB5) - * - IO 2 (PB6) - * - IO 3 (PB8) // NOTE: only usable on v3.1+ - * - IO 4 (PB7) - */ - -/***************Includes**********************/ -#include "msg_downlink_generated.h" -#include "msg_fc_update_generated.h" -#include "msg_uplink_generated.h" -#include "json_messages.h" - -#include "mbed.h" -#include "pins.h" - -#include "RFM69/RFM69.hpp" -#include "USBSerial.h" -#include -#include -#include -#include - -using namespace flatbuffers; -using namespace Calstar; - -/****************Defines**********************/ -#define ENCRYPT_KEY ("CALSTARENCRYPTKE") - -#define COMMAND_YES_RETRY ("![YES_RETRY]!") -#define COMMAND_NO_RETRY ("![NO_RETRY]!") - -#define LED_ON_TIME_MS (50) - -#define RX_BUF_LEN (256) -#define FLATBUF_BUF_SIZE (256) - -// resend messages for which have not received acks at 200ms intervals -#define ACK_CHECK_INTERVAL_MS (200) -#define MAX_NUM_RETRIES (50) - -#define BATUINT_TO_FLOAT(a) (((float)(a)) / 3308.4751259079328064817304607171f) - -// If enabled, turns off other debug printings -#define JSON_LOGGING - -/****************Global Variables***************/ -DigitalOut rx_led(LED_RX); -DigitalOut tx_led(LED_TX); -DigitalOut tx_lock(IO2); // using for tx lock led - -DigitalIn io1(IO1); -// DigitalIn io2(IO2); -DigitalIn io3(IO3); -DigitalIn io4(IO4); -Timer t; - -USBSerial pc; - -uint8_t rx_buf[RX_BUF_LEN]; -RFM69 radio(SPI1_MOSI, SPI1_MISO, SPI1_SCLK, SPI1_SSEL, RADIO_RST, true); - -std::string line = ""; -bool retry = true; - -int32_t t_tx_led_on; -int32_t t_rx_led_on; -int32_t t_last_resend; - -uint8_t frame_id; - -uint32_t total_bytes_sent; - -// frame_id, -std::unordered_map, uint8_t>> acks_remaining; - -FlatBufferBuilder builder(FLATBUF_BUF_SIZE); - -/***************Function Declarations***********/ -void start(); -void loop(); -bool sendUplinkMsg(const std::string &str, bool with_ack); -const DownlinkMsg *getDownlinkMsgChar(char c); -const DownlinkMsg *getDownlinkMsg(uint8_t *data, int32_t data_len); -void resend_msgs(); -void sendAck(uint8_t frame_id); -void radioTx(const uint8_t *const data, const int32_t data_len); - -void sendJsonInt(uint64_t timestamp, const std::string &id, int value); -void sendJsonFloat(uint64_t timestamp, const std::string &id, float value); -void sendJsonString(uint64_t timestamp, const std::string &id, const std::string &value); -void sendJsonLog(const std::string &value); - -int main() { - start(); - while (true) { - loop(); - } - return 0; -} - -void start() { - rx_led = 0; - tx_led = 0; - tx_lock = 0; - - radio.reset(); - #ifndef JSON_LOGGING - pc.printf("![Radio reset complete.]!\r\n"); - #endif - - radio.init(); - radio.setAESEncryption(ENCRYPT_KEY, strlen(ENCRYPT_KEY)); - - radio.setHighPowerSettings(true); - radio.setPowerDBm(20); - - t.start(); - t_tx_led_on = t.read_ms(); - t_rx_led_on = t.read_ms(); - - frame_id = 0; - - t_last_resend = t.read_ms(); - - // Input button set to internal pull-up - // It is now active low - io1.mode(PullUp); - - total_bytes_sent = 0; -} - -void loop() { - if (t.read_ms() - t_last_resend > ACK_CHECK_INTERVAL_MS) { - resend_msgs(); - t_last_resend = t.read_ms(); - } - if (tx_led.read() == 1 && t.read_ms() - t_tx_led_on > LED_ON_TIME_MS) { - tx_led = 0; - } - if (rx_led.read() == 1 && t.read_ms() - t_rx_led_on > LED_ON_TIME_MS) { - rx_led = 0; - } - if (io1) { - tx_lock = 1; - } else { - tx_lock = 0; - } - - if (pc.readable()) { - if (!io1) { - // Button is active-low - // We want to transmit nothing unless the button pressed - const char in = pc.getc(); - if (in == '\n') { - if (line == COMMAND_YES_RETRY) { - retry = true; - #ifndef JSON_LOGGING - pc.printf("%s\r\n", line.c_str()); - #endif - } else if (line == COMMAND_NO_RETRY) { - retry = false; - #ifndef JSON_LOGGING - pc.printf("%s\r\n", line.c_str()); - #endif - } else { - if (retry) { - #ifndef JSON_LOGGING - pc.printf("![SENDING WITH RETRY '%s', " - "bytes: %d]!\r\n", - line.c_str(), line.length()); - #endif - line += '\n'; - tx_led = 1; - sendUplinkMsg(line, true); - t_tx_led_on = t.read_ms(); - #ifndef JSON_LOGGING - pc.printf("\r\nOut of send up link " - "message true\r\n"); - #endif - } else { - #ifndef JSON_LOGGING - pc.printf("![SENDING ONCE ' %s ', " - "bytes: %d]!\r\n", - line.c_str(), line.length()); - #endif - line += '\n'; - tx_led = 1; - pc.putc('b'); - sendUplinkMsg(line, false); - t_tx_led_on = t.read_ms(); - } - } - line = ""; - } else { - line += in; - } - } else { - if (pc.getc() == '\n') { - #ifndef JSON_LOGGING - pc.printf("![TRANSMIT LOCKED]!\r\n"); - #endif - sendJsonLog("Failed to send frame: Transmit locked."); - } - } - } - - const int32_t num_bytes_rxd = radio.receive((char *)rx_buf, sizeof(rx_buf)); - if (num_bytes_rxd > 1) { - rx_buf[num_bytes_rxd] = '\0'; - rx_led = 1; - t_rx_led_on = t.read_ms(); - bool failed = true; - for (int32_t i = 0; i < num_bytes_rxd - 1; ++i) { - const DownlinkMsg *msg = getDownlinkMsgChar(rx_buf[i + 1]); - if (msg != nullptr) { - failed = false; - #ifndef JSON_LOGGING - pc.printf("![RSSI=%d, bytes: %d]!", radio.getRSSI(), num_bytes_rxd - 1); - #endif - - sendJsonInt(msg->TimeStamp(), GS_RSSI, radio.getRSSI()); - - if (msg->Type() == DownlinkType_Ack) { - if (acks_remaining.count(msg->FrameID()) == 1) { - acks_remaining.erase(msg->FrameID()); - } - #ifndef JSON_LOGGING - pc.printf("\r\n"); - #endif - } else if (msg->Type() == DownlinkType_StateUpdate) { - #ifndef JSON_LOGGING - pc.printf( - "tstamp: %" PRIu64 ", bytes: %d, state: %d, fc.pwr: " - "%d, " - "gps string: \"%s\", bat.v: %f", - msg->TimeStamp(), (int)msg->Bytes(), (int)msg->State(), - (int)msg->FCPowered(), msg->GPSString()->str().c_str(), - BATUINT_TO_FLOAT(msg->BattVoltage())); - #endif - - - sendJsonInt(msg->TimeStamp(), COMMS_RECD, msg->Bytes()); - sendJsonInt(msg->TimeStamp(), TPC_STATE, (int)msg->State()); - sendJsonInt(msg->TimeStamp(), FC_PWRD, (int)msg->FCPowered()); - sendJsonString(msg->TimeStamp(), TPC_GPS, msg->GPSString()->c_str()); - sendJsonFloat(msg->TimeStamp(), TPC_BATV, BATUINT_TO_FLOAT(msg->BattVoltage())); - - if (msg->FCMsg()) { - const FCUpdateMsg *fc = msg->FCMsg(); - - sendJsonInt(msg->TimeStamp(), FC_STATE, (int)fc->State()); - sendJsonFloat(msg->TimeStamp(), FC_ALT, fc->Altitude()); - - #ifndef JSON_LOGGING - pc.printf(", ((state: %d, alt: %f, " - "bps: %d %d, %d %d, %d %d, %d %d, %d %d, %d %d, %d %d))", - (int)fc->State(), fc->Altitude(), - (int)fc->BP1Continuity(), (int)fc->BP1Ignited(), - (int)fc->BP2Continuity(), (int)fc->BP2Ignited(), - (int)fc->BP3Continuity(), (int)fc->BP3Ignited(), - (int)fc->BP4Continuity(), (int)fc->BP4Ignited(), - (int)fc->BP5Continuity(), (int)fc->BP5Ignited(), - (int)fc->BP6Continuity(), (int)fc->BP6Ignited(), - (int)fc->BP7Continuity(), (int)fc->BP7Ignited()); - // pc.printf(", ((state: %d, accel: %f, " - // "%f %f, mag: %f, %f, %f, " - // "gyro: %f, " - // "%f, %f, alt: %f, " - // "pressure: %f, bps: %d %d, " - // "%d %d, %d %d, %d " - // "%d, %d %d, %d %d, %d %d))", - // (int)fc->State(), fc->AccelX(), fc->AccelY(), - // fc->AccelZ(), fc->MagX(), fc->MagY(), - // fc->MagZ(), fc->GyroX(), fc->GyroY(), - // fc->GyroZ(), fc->Altitude(), fc->Pressure(), - // (int)msg->FCMsg()->BP1Continuity(), - // (int)msg->FCMsg()->BP1Ignited(), - // (int)msg->FCMsg()->BP2Continuity(), - // (int)msg->FCMsg()->BP2Ignited(), - // (int)msg->FCMsg()->BP3Continuity(), - // (int)msg->FCMsg()->BP3Ignited(), - // (int)msg->FCMsg()->BP4Continuity(), - // (int)msg->FCMsg()->BP4Ignited(), - // (int)msg->FCMsg()->BP5Continuity(), - // (int)msg->FCMsg()->BP5Ignited(), - // (int)msg->FCMsg()->BP6Continuity(), - // (int)msg->FCMsg()->BP6Ignited(), - // (int)msg->FCMsg()->BP7Continuity(), - // (int)msg->FCMsg()->BP7Ignited()); - #endif - } - #ifndef JSON_LOGGING - pc.printf("\r\n"); - #endif - } - if (msg->AckReqd()) { - sendAck(msg->FrameID()); - } - } else { - } - } - if (failed) { - // pc.printf("[!RSSI=%d, bytes: %d]! Failed Flatbuf - // Deserialize\r\n", - // radio.getRSSI(), num_bytes_rxd - 1); - } - } -} - -bool sendUplinkMsg(const std::string &str, bool with_ack) { - builder.Reset(); - - uint8_t bps[7] = {0, 0, 0, 0, 0, 0, 0}; - /* NEED TO ACTUALLY PARSE STR */ - UplinkType type = UplinkType_FCOff; - if (str[0] == 'n') { - type = UplinkType_FCOn; - } else if (str[0] == 'b') { - type = UplinkType_BlackPowderPulse; - #ifndef JSON_LOGGING - pc.printf("%s\r\n", str.c_str()); - #endif - if (str.length() >= 8) { - for (int32_t i = 1; i < 8; ++i) { - if (str[i] == '1') { - bps[i - 1] = 1; - } else { - bps[i - 1] = 0; - } - } - } - } else if (str[0] == 'f') { - type = UplinkType_FCOff; - } else { - type = UplinkType_FCOff; - } - - auto blackpowder_offset = builder.CreateVector(bps, sizeof(bps)); - - Offset msg = CreateUplinkMsg(builder, 1, type, blackpowder_offset, frame_id, with_ack); - builder.Finish(msg); - - const uint8_t bytes = (uint8_t)builder.GetSize(); - builder.Reset(); - blackpowder_offset = builder.CreateVector(bps, sizeof(bps)); - msg = CreateUplinkMsg(builder, bytes, type, blackpowder_offset, frame_id, with_ack); - builder.Finish(msg); - - const uint8_t *buf = builder.GetBufferPointer(); - const int32_t size = builder.GetSize(); - - if (with_ack) { - acks_remaining.insert({frame_id, {std::vector(buf, buf + size), 0}}); - } - radioTx(buf, size); - - ++frame_id; -} - -uint8_t ret_buf[FLATBUF_BUF_SIZE]; -const DownlinkMsg *getDownlinkMsgChar(char c) { - static uint8_t buffer[FLATBUF_BUF_SIZE]; - static unsigned int len = 0; - - if (len == FLATBUF_BUF_SIZE) { - // If at end of buffer, shift and add to end - memmove(buffer, buffer + 1, FLATBUF_BUF_SIZE - 1); - buffer[FLATBUF_BUF_SIZE - 1] = (uint8_t)c; - } else { - // Otherwise build up buffer - buffer[len++] = (uint8_t)c; - } - - // The verifier will say that buf has a valid message for any length - // from actual_length-some number to full buffer length So basically, we - // trust the verifier, but verify separately by having a #-bytes field - // in the message itself So if the verifier says there's a valid message - // in the buffer, we read that message, get the number of bytes that the - // message says it should be, and actually process a message of THAT - // size. - Verifier verifier(buffer, len); - if (VerifyDownlinkMsgBuffer(verifier)) { - const DownlinkMsg *msg = GetDownlinkMsg(buffer); - // The message knows how big it should be - const uint8_t expectedBytes = msg->Bytes(); - - uint8_t actual_len = len; - if (len < expectedBytes) { - // The verifier will say we have a valid message even if - // we're a few bytes short Just read more characters at - // this point by returning early - return nullptr; - } else if (len > expectedBytes) { - // Now we want to verify that the "smaller buffer" with - // length equal to the expected number of bytes is - // actually a message in its own right (just a double - // check basically) - Verifier smallerVerifier(buffer, expectedBytes); - if (VerifyDownlinkMsgBuffer(smallerVerifier)) { - // If it is a message, then make sure we use the - // correct (smaller) length - actual_len = expectedBytes; - } else { - // If it isn't valid, then this buffer just has - // some malformed messages... continue and let's - // get them out of the buffer by reading more - return nullptr; - } - } - - // Now that we've read a valid message, copy it into the output - // buffer, then remove it from the input buffer and move - // everything else down. Then reduce current buffer length by - // the length of the processed message Then clear the rest of - // the buffer so that we don't get false positives with the - // verifiers - memcpy(ret_buf, buffer, actual_len); - memmove(buffer, buffer + actual_len, FLATBUF_BUF_SIZE - actual_len); - len -= actual_len; - // Clear the rest of the buffer - memset(buffer + len, 0, FLATBUF_BUF_SIZE - len); - - return GetDownlinkMsg(ret_buf); - } - return nullptr; -} - -void resend_msgs() { - for (auto &msg : acks_remaining) { - tx_led = 1; - #ifndef JSON_LOGGING - pc.printf("![RESENDING FRAME '%d']!\r\n", (int)msg.first); - #endif - const std::vector &vec = std::get<0>(msg.second); - radioTx(vec.data(), vec.size()); - std::get<1>(msg.second) = std::get<1>(msg.second) + 1; - if (std::get<1>(msg.second) >= MAX_NUM_RETRIES) { - #ifndef JSON_LOGGING - pc.printf("![FAILED TO SEND FRAME '%d']!\r\n", msg.first); - #endif - sendJsonLog("Failed to send frame: Exceeded max number of retries."); - acks_remaining.erase(msg.first); - } - } - if (tx_led.read() == 1) { - t_tx_led_on = t.read_ms(); - } -} - -void sendAck(uint8_t frame_id) { - builder.Reset(); - Offset ack = CreateUplinkMsg(builder, 1, UplinkType_Ack, 0, frame_id, false); - builder.Finish(ack); - const uint8_t bytes = builder.GetSize(); - builder.Reset(); - ack = CreateUplinkMsg(builder, bytes, UplinkType_Ack, 0, frame_id, false); - builder.Finish(ack); - radioTx(builder.GetBufferPointer(), builder.GetSize()); -} - - -void sendJsonInt(uint64_t timestamp, const std::string &id, int value) { - pc.printf("{ \"timestamp\": %" PRIu64 ", \"id\": \"%s\", \"value\": %d}\r\n", - timestamp, id.c_str(), value); -} - -void sendJsonFloat(uint64_t timestamp, const std::string &id, float value) { - pc.printf("{ \"timestamp\": %" PRIu64 ", \"id\": \"%s\", \"value\": %f}\r\n", - timestamp, id.c_str(), value); -} - -void sendJsonString(uint64_t timestamp, const std::string &id, const std::string &value) { - pc.printf("{ \"timestamp\": %" PRIu64 ", \"id\": \"%s\", \"value\": \"%s\"}\r\n", - timestamp, id.c_str(), value.c_str()); -} - -void sendJsonLog(const std::string &value) { - pc.printf("{\"timestamp\": -1, \"id\": \"%s\", \"value\": \"%s\"}\r\n", - GS_LOG, value.c_str()); -} - -void radioTx(const uint8_t *const data, const int32_t data_len) { - radio.send(data, data_len); - total_bytes_sent += data_len; - pc.printf("{ \"timestamp\": -1, \"id\": \"%s\", \"value\": %d}\r\n", - COMMS_SENT, total_bytes_sent); -} +/* + * CalSTAR Avionics Ground Station + * + * file: gs.h + * + * Provides communication between laptop and rocket. + * Communicates with laptop via UART over USB (with UART with an FTDI cable as + * backup) Communicates with rocket via radio, 433 MHz Has 2 configurable LEDs + * and 4 digital inputs on terminal blocks + * + * LEDS: + * - Rx (PB12) + * - Tx (PB13) + * + * Digital Inputs: + * - IO 1 (PB5) + * - IO 2 (PB6) + * - IO 3 (PB8) // NOTE: only usable on v3.1+ + * - IO 4 (PB7) + */ + +/* Includes */ + +#include "pins.h" + +#include "mbed.h" +#include "USBSerial.h" +#include "RFM69/RFM69.hpp" + +#include "msg_downlink_generated.h" +#include "msg_fc_update_generated.h" +#include "msg_uplink_generated.h" + +using namespace Calstar; + +#include +#include +#include +#include + +/* Defines */ + +// Must be exactly 16 bytes (excluding null-terminator). +// Therefore is missing the 'Y' on purpose. +#define ENCRYPT_KEY ("CALSTARENCRYPTKE") + +#define LED_ON_TIME_MS (50) + +#define RX_BUF_LEN (256) + +#define ACK_RESEND_INTERVAL_MS (200) + +/* Function Declarations */ +void init_leds(); +void init_radio(); +void init_inputs(); + +void update_leds(); +void update_inputs(); + +void resend_missing_acks(); + +void execute_command(const std::string &cmd); + +const DownlinkMsg *parse_rx_buf(uint8_t *buf, size_t buf_size); +void handle_incoming_msg(const DownlinkMsg *msg); +void update_acks(uint8_t frame_id); +void send_ack(uint8_t frame_id); + +void json_log_to_serial(const DownlinkMsg *msg); + +/* Global Variables */ +Timer t; + +DigitalOut rx_led(LED_RX); +DigitalOut tx_led(LED_TX); +DigitalOut tx_lock_led(IO2); +int32_t t_tx_led_on; +int32_t t_rx_led_on; + +DigitalIn tx_lock_button(IO1); + +uint32_t total_bytes_sent; + +uint8_t tx_frame_id; + +int32_t t_last_ack_resend; + +uint8_t rx_buf[RX_BUF_LEN]; +RFM69 radio(SPI1_MOSI, SPI1_MISO, SPI1_SCLK, SPI1_SSEL, RADIO_RST, true); + +USBSerial serial; + +std::string line; + +struct TelemetryMsg { + std::vector buf; + uint8_t frame_id; +}; + +std::unordered_map acks_remaining; + +/* Function Definitions */ +void start() { + t.start(); + + init_leds(); + init_radio(); + init_inputs(); + + tx_frame_id = 0; + total_bytes_sent = 0; + t_last_ack_resend = t.read_ms(); +} + +void loop() { + update_leds(); + update_inputs(); + + if (t.read_ms() - t_last_ack_resend > ACK_RESEND_INTERVAL_MS) { + resend_missing_acks(); + t_last_ack_resend = t.read_ms(); + } + + if (serial.readable()) { + const char c = serial.getc(); + if (c == '\n') { + execute_command(line); + } else { + line += c; + } + } + + const int32_t num_bytes_rxd = radio.receive((char *)rx_buf, sizeof(rx_buf)); + if (num_bytes_rxd > 1) { + rx_buf[num_bytes_rxd] = '\0'; + + rx_led = 1; + t_rx_led_on = t.read_ms(); + + const DownlinkMsg *msg = parse_rx_buf(rx_buf + 1, sizeof(rx_buf) - 1); + if (msg != nullptr) { + handle_incoming_msg(msg); + } + } +} + +void init_leds() { + rx_led = 0; + tx_led = 0; + tx_lock_led = 0; + + int32_t t0 = t.read_ms(); + t_tx_led_on = t0; + t_rx_led_on = t0; +} + +void init_radio() { + radio.reset(); + radio.init(); + radio.setAESEncryption(ENCRYPT_KEY, strlen(ENCRYPT_KEY)); + radio.setHighPowerSettings(true); + radio.setPowerDBm(20); +} + +void init_inputs() { + // Set to active-low using internal pull-up + tx_lock_button.mode(PullUp); +} + +void update_leds() { + if (tx_led.read() == 1 && t.read_ms() - t_tx_led_on > LED_ON_TIME_MS) { + tx_led = 0; + } + if (rx_led.read() == 1 && t.read_ms() - t_rx_led_on > LED_ON_TIME_MS) { + rx_led = 0; + } +} + +void update_inputs() { + if (tx_lock_button) { + tx_lock_led = 1; + } else { + tx_lock_led = 0; + } +} + +void resend_missing_acks() { + +} + +void execute_command(const std::string &cmd) { + +} + +const DownlinkMsg *parse_rx_buf(uint8_t *buf, size_t buf_size) { + return nullptr; +} + +void handle_incoming_msg(const DownlinkMsg *msg) { + if (msg->AckReqd()) { + send_ack(msg->FrameID()); + } + + switch (msg->Type()) { + case DownlinkType_Ack: + update_acks(msg->FrameID()); + break; + case DownlinkType_StateUpdate: + json_log_to_serial(msg); + break; + default: + break; + } +} + +void send_ack(uint8_t frame_id) { + +} + +void update_acks(uint8_t frame_id) { + if (acks_remaining.count(frame_id) >= 1) { + acks_remaining.erase(frame_id); + } +} + +void json_log_to_serial(const DownlinkMsg *msg) { + +} + +int main() { + start(); + while (true) { + loop(); + } + return 0; +} \ No newline at end of file From 015790cbc0016cba796745e2ffb1c7acb5dca579 Mon Sep 17 00:00:00 2001 From: Neelay Junnarkar Date: Thu, 25 Apr 2019 12:51:43 -0700 Subject: [PATCH 2/6] more refactoring --- gs/gs.h | 139 ++++++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 124 insertions(+), 15 deletions(-) diff --git a/gs/gs.h b/gs/gs.h index 99f622c..13fe00e 100644 --- a/gs/gs.h +++ b/gs/gs.h @@ -31,6 +31,7 @@ #include "msg_fc_update_generated.h" #include "msg_uplink_generated.h" +using namespace flatbuffers; using namespace Calstar; #include @@ -38,6 +39,12 @@ using namespace Calstar; #include #include +struct TelecmdMsg { + std::vector buf; + uint8_t frame_id; + uint8_t num_retries; +}; + /* Defines */ // Must be exactly 16 bytes (excluding null-terminator). @@ -49,6 +56,12 @@ using namespace Calstar; #define RX_BUF_LEN (256) #define ACK_RESEND_INTERVAL_MS (200) +#define MAX_NUM_RETRIES (50) + +#define COMMAND_YES_RETRY ("![YES_RETRY]!") +#define COMMAND_NO_RETRY ("![NO_RETRY]!") + +#define FLATBUF_BUF_SIZE (256) /* Function Declarations */ void init_leds(); @@ -60,11 +73,14 @@ void update_inputs(); void resend_missing_acks(); -void execute_command(const std::string &cmd); +bool execute_command(const std::string &cmd); +bool parse_to_telecmd_msg(const std::string &line, uint8_t frame_id, bool ack_req, std::vector *const out); + +void radio_transmit(const uint8_t *const buf, const int32_t size); -const DownlinkMsg *parse_rx_buf(uint8_t *buf, size_t buf_size); +const DownlinkMsg *parse_to_downlink(uint8_t *buf, size_t buf_size); void handle_incoming_msg(const DownlinkMsg *msg); -void update_acks(uint8_t frame_id); +void handle_acks(uint8_t frame_id); void send_ack(uint8_t frame_id); void json_log_to_serial(const DownlinkMsg *msg); @@ -88,17 +104,15 @@ int32_t t_last_ack_resend; uint8_t rx_buf[RX_BUF_LEN]; RFM69 radio(SPI1_MOSI, SPI1_MISO, SPI1_SCLK, SPI1_SSEL, RADIO_RST, true); +bool radio_retry; USBSerial serial; std::string line; -struct TelemetryMsg { - std::vector buf; - uint8_t frame_id; -}; +std::unordered_map acks_remaining; -std::unordered_map acks_remaining; +FlatBufferBuilder builder(FLATBUF_BUF_SIZE); /* Function Definitions */ void start() { @@ -111,6 +125,8 @@ void start() { tx_frame_id = 0; total_bytes_sent = 0; t_last_ack_resend = t.read_ms(); + radio_retry = true; + line = ""; } void loop() { @@ -125,7 +141,10 @@ void loop() { if (serial.readable()) { const char c = serial.getc(); if (c == '\n') { - execute_command(line); + if (!execute_command(line)) { + // log that execute failed + } + line = ""; } else { line += c; } @@ -138,7 +157,7 @@ void loop() { rx_led = 1; t_rx_led_on = t.read_ms(); - const DownlinkMsg *msg = parse_rx_buf(rx_buf + 1, sizeof(rx_buf) - 1); + const DownlinkMsg *msg = parse_to_downlink(rx_buf + 1, sizeof(rx_buf) - 1); if (msg != nullptr) { handle_incoming_msg(msg); } @@ -186,14 +205,97 @@ void update_inputs() { } void resend_missing_acks() { + for (auto &kv : acks_remaining) { + TelecmdMsg &msg = kv.second; + radio_transmit(msg.buf.data(), msg.buf.size()); + ++msg.num_retries; + if (msg.num_retries >= MAX_NUM_RETRIES) { + acks_remaining.erase(msg.frame_id); + // log that failed to send. + } + } +} + +bool execute_command(const std::string &cmd) { + if (line == COMMAND_YES_RETRY) { + radio_retry = true; + // log that retry is set + } else if (line == COMMAND_NO_RETRY) { + radio_retry = false; + // log that retry is unset + } else if (!tx_lock_button) { + line += '\n'; + + TelecmdMsg msg; + msg.frame_id = tx_frame_id; + msg.num_retries = 0; + if (!parse_to_telecmd_msg(cmd, msg.frame_id, radio_retry, &msg.buf)) { + return false; + } + ++tx_frame_id; + if (radio_retry) { + acks_remaining.insert({msg.frame_id, msg}); + } + radio_transmit(msg.buf.data(), msg.buf.size()); + } + return true; } -void execute_command(const std::string &cmd) { +bool parse_to_telecmd_msg(const std::string &line, uint8_t frame_id, bool ack_req, std::vector *const out) { + UplinkType type = UplinkType_FCOff; + uint8_t bps[7] = {0, 0, 0, 0, 0, 0, 0}; + + if (line[0] == 'n') { + type = UplinkType_FCOn; + } else if (line[0] == 'o') { + type = UplinkType_FCOff; + } else if (line[0] == 'b') { + type = UplinkType_BlackPowderPulse; + if (line.length() >= 8) { + for (int32_t i = 1; i < 8; ++i) { + if (line[i] == '1') { + bps[i - 1] = 1; + } else if (line[i] == '0') { + bps[i - 1] = 0; + } else { + return false; + } + } + } else { + return false; + } + } else { + return false; + } + + builder.Reset(); + auto bps_offset = builder.CreateVector(bps, sizeof(bps)); + Offset msg = CreateUplinkMsg(builder, 1, type, bps_offset, frame_id, ack_req); + builder.Finish(msg); + + const uint8_t bytes = (uint8_t)builder.GetSize(); + builder.Reset(); + bps_offset = builder.CreateVector(bps, sizeof(bps)); + msg = CreateUplinkMsg(builder, bytes, type, bps_offset, frame_id, ack_req); + builder.Finish(msg); + + const uint8_t *const buf = builder.GetBufferPointer(); + const int32_t size = builder.GetSize(); + + *out = std::vector(buf, buf + size); + return true; } -const DownlinkMsg *parse_rx_buf(uint8_t *buf, size_t buf_size) { +void radio_transmit(const uint8_t *const buf, const int32_t size) { + tx_led = 1; + radio.send(buf, size); + t_tx_led_on = t.read_ms(); + total_bytes_sent += size; +} + +const DownlinkMsg *parse_to_downlink(uint8_t *buf, size_t buf_size) { return nullptr; } @@ -204,7 +306,7 @@ void handle_incoming_msg(const DownlinkMsg *msg) { switch (msg->Type()) { case DownlinkType_Ack: - update_acks(msg->FrameID()); + handle_acks(msg->FrameID()); break; case DownlinkType_StateUpdate: json_log_to_serial(msg); @@ -215,10 +317,17 @@ void handle_incoming_msg(const DownlinkMsg *msg) { } void send_ack(uint8_t frame_id) { - + builder.Reset(); + Offset ack = CreateUplinkMsg(builder, 1, UplinkType_Ack, 0, frame_id, false); + builder.Finish(ack); + const uint8_t bytes = builder.GetSize(); + builder.Reset(); + ack = CreateUplinkMsg(builder, bytes, UplinkType_Ack, 0, frame_id, false); + builder.Finish(ack); + radio_transmit(builder.GetBufferPointer(), builder.GetSize()); } -void update_acks(uint8_t frame_id) { +void handle_acks(uint8_t frame_id) { if (acks_remaining.count(frame_id) >= 1) { acks_remaining.erase(frame_id); } From 7283d5e49f9a48689e1b7f0b0d29fe416e2c5de3 Mon Sep 17 00:00:00 2001 From: Neelay Junnarkar Date: Thu, 25 Apr 2019 17:19:01 -0700 Subject: [PATCH 3/6] downlink verification --- gs/gs.h | 80 +++++++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 67 insertions(+), 13 deletions(-) diff --git a/gs/gs.h b/gs/gs.h index 13fe00e..bbab0db 100644 --- a/gs/gs.h +++ b/gs/gs.h @@ -74,16 +74,19 @@ void update_inputs(); void resend_missing_acks(); bool execute_command(const std::string &cmd); -bool parse_to_telecmd_msg(const std::string &line, uint8_t frame_id, bool ack_req, std::vector *const out); +bool parse_to_telecmd_msg(const std::string &line, const uint8_t frame_id, const bool ack_req, std::vector *const out); void radio_transmit(const uint8_t *const buf, const int32_t size); -const DownlinkMsg *parse_to_downlink(uint8_t *buf, size_t buf_size); -void handle_incoming_msg(const DownlinkMsg *msg); -void handle_acks(uint8_t frame_id); -void send_ack(uint8_t frame_id); +const DownlinkMsg *stream_parse_to_downlink(const uint8_t *const buf, const size_t buf_size); +void push_to_stream(const uint8_t b, uint8_t *const buf, size_t *const buf_size); +const DownlinkMsg *parse_to_downlink(uint8_t *const buf, size_t *const buf_size); -void json_log_to_serial(const DownlinkMsg *msg); +void handle_incoming_msg(const DownlinkMsg *const msg); +void handle_acks(const uint8_t frame_id); +void send_ack(const uint8_t frame_id); + +void json_log_to_serial(const DownlinkMsg *const msg); /* Global Variables */ Timer t; @@ -113,6 +116,7 @@ std::string line; std::unordered_map acks_remaining; FlatBufferBuilder builder(FLATBUF_BUF_SIZE); +uint8_t out_buf[FLATBUF_BUF_SIZE]; /* Function Definitions */ void start() { @@ -157,7 +161,7 @@ void loop() { rx_led = 1; t_rx_led_on = t.read_ms(); - const DownlinkMsg *msg = parse_to_downlink(rx_buf + 1, sizeof(rx_buf) - 1); + const DownlinkMsg *msg = stream_parse_to_downlink(rx_buf + 1, sizeof(rx_buf) - 1); if (msg != nullptr) { handle_incoming_msg(msg); } @@ -242,7 +246,7 @@ bool execute_command(const std::string &cmd) { return true; } -bool parse_to_telecmd_msg(const std::string &line, uint8_t frame_id, bool ack_req, std::vector *const out) { +bool parse_to_telecmd_msg(const std::string &line, const uint8_t frame_id, const bool ack_req, std::vector *const out) { UplinkType type = UplinkType_FCOff; uint8_t bps[7] = {0, 0, 0, 0, 0, 0, 0}; @@ -295,11 +299,61 @@ void radio_transmit(const uint8_t *const buf, const int32_t size) { total_bytes_sent += size; } -const DownlinkMsg *parse_to_downlink(uint8_t *buf, size_t buf_size) { +const DownlinkMsg *stream_parse_to_downlink(const uint8_t *const buf, const size_t buf_size) { + static uint8_t stream[FLATBUF_BUF_SIZE]; + static size_t stream_len = 0; + + const DownlinkMsg *msg = nullptr; + for (size_t i = 0; i < buf_size; ++i) { + push_to_stream(buf[i], stream, &stream_len); + + msg = parse_to_downlink(stream, &stream_len); + if (msg != nullptr) { + return msg; + } + } + return nullptr; +} + +void push_to_stream(const uint8_t b, uint8_t *const buf, size_t *const buf_size) { + if (*buf_size == FLATBUF_BUF_SIZE) { + memmove(buf, buf + 1, FLATBUF_BUF_SIZE - 1); + buf[FLATBUF_BUF_SIZE - 1] = b; + } else { + buf[*buf_size] = b; + ++(*buf_size); + } +} + +const DownlinkMsg *parse_to_downlink(uint8_t *const buf, size_t *const buf_size) { + Verifier verifier(buf, *buf_size); + if (VerifyDownlinkMsgBuffer(verifier)) { + const DownlinkMsg *const msg = GetDownlinkMsg(buf); + const uint8_t expected_num_bytes = msg->Bytes(); + + uint8_t actual_size = *buf_size; + if (*buf_size < expected_num_bytes) { + return nullptr; + } else if (*buf_size > expected_num_bytes) { + Verifier smaller_verifier(buf, expected_num_bytes); + if (VerifyDownlinkMsgBuffer(smaller_verifier)) { + actual_size = expected_num_bytes; + } else { + return nullptr; + } + } + + memcpy(out_buf, buf, actual_size); + memmove(buf, buf + actual_size, FLATBUF_BUF_SIZE - actual_size); + *buf_size -= actual_size; + memset(buf + *buf_size, 0, FLATBUF_BUF_SIZE - *buf_size); + + return GetDownlinkMsg(out_buf); + } return nullptr; } -void handle_incoming_msg(const DownlinkMsg *msg) { +void handle_incoming_msg(const DownlinkMsg *const msg) { if (msg->AckReqd()) { send_ack(msg->FrameID()); } @@ -316,7 +370,7 @@ void handle_incoming_msg(const DownlinkMsg *msg) { } } -void send_ack(uint8_t frame_id) { +void send_ack(const uint8_t frame_id) { builder.Reset(); Offset ack = CreateUplinkMsg(builder, 1, UplinkType_Ack, 0, frame_id, false); builder.Finish(ack); @@ -327,13 +381,13 @@ void send_ack(uint8_t frame_id) { radio_transmit(builder.GetBufferPointer(), builder.GetSize()); } -void handle_acks(uint8_t frame_id) { +void handle_acks(const uint8_t frame_id) { if (acks_remaining.count(frame_id) >= 1) { acks_remaining.erase(frame_id); } } -void json_log_to_serial(const DownlinkMsg *msg) { +void json_log_to_serial(const DownlinkMsg *const msg) { } From 987f18d0f546655dd70cfdba95be977ba027c80a Mon Sep 17 00:00:00 2001 From: Neelay Junnarkar Date: Thu, 25 Apr 2019 22:56:18 -0700 Subject: [PATCH 4/6] added logging --- gs/gs.h | 99 +++++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 79 insertions(+), 20 deletions(-) diff --git a/gs/gs.h b/gs/gs.h index bbab0db..e665b5a 100644 --- a/gs/gs.h +++ b/gs/gs.h @@ -26,7 +26,7 @@ #include "mbed.h" #include "USBSerial.h" #include "RFM69/RFM69.hpp" - +#include "json_messages.h" #include "msg_downlink_generated.h" #include "msg_fc_update_generated.h" #include "msg_uplink_generated.h" @@ -55,6 +55,8 @@ struct TelecmdMsg { #define RX_BUF_LEN (256) +#define JSON_LOG_UPDATE_INTERVAL_MS (200) + #define ACK_RESEND_INTERVAL_MS (200) #define MAX_NUM_RETRIES (50) @@ -63,6 +65,8 @@ struct TelecmdMsg { #define FLATBUF_BUF_SIZE (256) +#define BATUINT_TO_FLOAT(a) (((float)(a)) / 3308.4751259079328064817304607171f) + /* Function Declarations */ void init_leds(); void init_radio(); @@ -71,6 +75,8 @@ void init_inputs(); void update_leds(); void update_inputs(); +void periodic_json_log_update(); + void resend_missing_acks(); bool execute_command(const std::string &cmd); @@ -88,6 +94,11 @@ void send_ack(const uint8_t frame_id); void json_log_to_serial(const DownlinkMsg *const msg); +void sendJsonInt(uint64_t timestamp, const std::string &id, int value); +void sendJsonFloat(uint64_t timestamp, const std::string &id, float value); +void sendJsonString(uint64_t timestamp, const std::string &id, const std::string &value); +void sendJsonLog(const std::string &value); + /* Global Variables */ Timer t; @@ -104,6 +115,7 @@ uint32_t total_bytes_sent; uint8_t tx_frame_id; int32_t t_last_ack_resend; +int32_t t_last_periodic_log; uint8_t rx_buf[RX_BUF_LEN]; RFM69 radio(SPI1_MOSI, SPI1_MISO, SPI1_SCLK, SPI1_SSEL, RADIO_RST, true); @@ -129,14 +141,21 @@ void start() { tx_frame_id = 0; total_bytes_sent = 0; t_last_ack_resend = t.read_ms(); + t_last_periodic_log = t.read_ms(); radio_retry = true; line = ""; + sendJsonLog("Start: Initialization complete."); } void loop() { update_leds(); update_inputs(); + if (t.read_ms() - t_last_periodic_log > JSON_LOG_UPDATE_INTERVAL_MS) { + periodic_json_log_update(); + t_last_periodic_log = t.read_ms(); + } + if (t.read_ms() - t_last_ack_resend > ACK_RESEND_INTERVAL_MS) { resend_missing_acks(); t_last_ack_resend = t.read_ms(); @@ -173,9 +192,10 @@ void init_leds() { tx_led = 0; tx_lock_led = 0; - int32_t t0 = t.read_ms(); - t_tx_led_on = t0; - t_rx_led_on = t0; + t_tx_led_on = t.read_ms(); + t_rx_led_on = t.read_ms(); + + sendJsonLog("Leds: Turned off."); } void init_radio() { @@ -184,11 +204,13 @@ void init_radio() { radio.setAESEncryption(ENCRYPT_KEY, strlen(ENCRYPT_KEY)); radio.setHighPowerSettings(true); radio.setPowerDBm(20); + sendJsonLog("Radio: Initialized."); } void init_inputs() { // Set to active-low using internal pull-up tx_lock_button.mode(PullUp); + sendJsonLog("Tx Lock Button: Pulled up"); } void update_leds() { @@ -208,6 +230,10 @@ void update_inputs() { } } +void periodic_json_log_update() { + sendJsonInt(-1, COMMS_SENT, total_bytes_sent); +} + void resend_missing_acks() { for (auto &kv : acks_remaining) { TelecmdMsg &msg = kv.second; @@ -215,7 +241,8 @@ void resend_missing_acks() { ++msg.num_retries; if (msg.num_retries >= MAX_NUM_RETRIES) { acks_remaining.erase(msg.frame_id); - // log that failed to send. + sendJsonLog("Failed to send frame " + std::to_string(msg.frame_id) + + ": Exceeded max number of retries."); } } } @@ -223,25 +250,30 @@ void resend_missing_acks() { bool execute_command(const std::string &cmd) { if (line == COMMAND_YES_RETRY) { radio_retry = true; - // log that retry is set + sendJsonLog("Radio: Set to transmit with retry."); } else if (line == COMMAND_NO_RETRY) { radio_retry = false; - // log that retry is unset - } else if (!tx_lock_button) { - line += '\n'; - - TelecmdMsg msg; - msg.frame_id = tx_frame_id; - msg.num_retries = 0; - if (!parse_to_telecmd_msg(cmd, msg.frame_id, radio_retry, &msg.buf)) { - return false; - } - ++tx_frame_id; + sendJsonLog("Radio: Set to transmit without retry."); + } else { + if (!tx_lock_button) { + line += '\n'; + + TelecmdMsg msg; + msg.frame_id = tx_frame_id; + msg.num_retries = 0; + if (!parse_to_telecmd_msg(cmd, msg.frame_id, radio_retry, &msg.buf)) { + sendJsonLog("Failed to send frame: unable to parse input."); + return false; + } + ++tx_frame_id; - if (radio_retry) { - acks_remaining.insert({msg.frame_id, msg}); + if (radio_retry) { + acks_remaining.insert({msg.frame_id, msg}); + } + radio_transmit(msg.buf.data(), msg.buf.size()); + } else { + sendJsonLog("Failed to send frame: Transmit locked."); } - radio_transmit(msg.buf.data(), msg.buf.size()); } return true; } @@ -388,7 +420,34 @@ void handle_acks(const uint8_t frame_id) { } void json_log_to_serial(const DownlinkMsg *const msg) { + const uint64_t tstamp = msg->TimeStamp(); + sendJsonInt( tstamp, COMMS_RECD, msg->Bytes()); + sendJsonInt( tstamp, TPC_STATE, (int)msg->State()); + sendJsonInt( tstamp, FC_PWRD, (int)msg->FCPowered()); + sendJsonString(tstamp, TPC_GPS, msg->GPSString()->c_str()); + sendJsonFloat( tstamp, TPC_BATV, BATUINT_TO_FLOAT(msg->BattVoltage())); + + if (msg->FCMsg()) { + const FCUpdateMsg *const fc = msg->FCMsg(); + sendJsonInt( tstamp, FC_STATE, (int)fc->State()); + sendJsonFloat(tstamp, FC_ALT, fc->Altitude()); + } +} + +void sendJsonInt(uint64_t timestamp, const std::string &id, int value) { + serial.printf("{ \"timestamp\": %" PRIu64 ", \"id\": \"%s\", \"value\": %d}\r\n", timestamp, id.c_str(), value); +} + +void sendJsonFloat(uint64_t timestamp, const std::string &id, float value) { + serial.printf("{ \"timestamp\": %" PRIu64 ", \"id\": \"%s\", \"value\": %f}\r\n", timestamp, id.c_str(), value); +} + +void sendJsonString(uint64_t timestamp, const std::string &id, const std::string &value) { + serial.printf("{ \"timestamp\": %" PRIu64 ", \"id\": \"%s\", \"value\": \"%s\"}\r\n", timestamp, id.c_str(), value.c_str()); +} +void sendJsonLog(const std::string &value) { + serial.printf("{\"timestamp\": -1, \"id\": \"%s\", \"value\": \"%s\"}\r\n", GS_LOG, value.c_str()); } int main() { From 980b46950176a0b30a35c3661e3f1a77f06c41a3 Mon Sep 17 00:00:00 2001 From: Neelay Junnarkar Date: Thu, 25 Apr 2019 23:00:20 -0700 Subject: [PATCH 5/6] minor changes --- gs/gs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gs/gs.h b/gs/gs.h index e665b5a..fcf767f 100644 --- a/gs/gs.h +++ b/gs/gs.h @@ -165,7 +165,7 @@ void loop() { const char c = serial.getc(); if (c == '\n') { if (!execute_command(line)) { - // log that execute failed + sendJsonLog("Failed to execute command."); } line = ""; } else { From aeedaffaf5543722c2185bff55885bc54ba72e10 Mon Sep 17 00:00:00 2001 From: Neelay Junnarkar Date: Sun, 28 Apr 2019 11:12:25 -0700 Subject: [PATCH 6/6] refactor works properly --- gs/gs.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/gs/gs.h b/gs/gs.h index fcf767f..edb36a5 100644 --- a/gs/gs.h +++ b/gs/gs.h @@ -180,7 +180,7 @@ void loop() { rx_led = 1; t_rx_led_on = t.read_ms(); - const DownlinkMsg *msg = stream_parse_to_downlink(rx_buf + 1, sizeof(rx_buf) - 1); + const DownlinkMsg *msg = stream_parse_to_downlink(rx_buf + 1, num_bytes_rxd - 1); if (msg != nullptr) { handle_incoming_msg(msg); } @@ -231,7 +231,7 @@ void update_inputs() { } void periodic_json_log_update() { - sendJsonInt(-1, COMMS_SENT, total_bytes_sent); + serial.printf("{ \"timestamp\": -1, \"id\": \"%s\", \"value\": %d}\r\n", COMMS_SENT, total_bytes_sent); } void resend_missing_acks() { @@ -358,11 +358,14 @@ void push_to_stream(const uint8_t b, uint8_t *const buf, size_t *const buf_size) } const DownlinkMsg *parse_to_downlink(uint8_t *const buf, size_t *const buf_size) { + // Check if some contiguous sub-sequence from the beginning is a valid + // (potentially partial) DownlinkMsg Verifier verifier(buf, *buf_size); if (VerifyDownlinkMsgBuffer(verifier)) { - const DownlinkMsg *const msg = GetDownlinkMsg(buf); + const DownlinkMsg *msg = GetDownlinkMsg(buf); const uint8_t expected_num_bytes = msg->Bytes(); + // Now use how big the message was expected to be to reverify uint8_t actual_size = *buf_size; if (*buf_size < expected_num_bytes) { return nullptr; @@ -375,9 +378,11 @@ const DownlinkMsg *parse_to_downlink(uint8_t *const buf, size_t *const buf_size) } } + // Copy valid message to an output buffer and remove relevant bytes from the stream buffer memcpy(out_buf, buf, actual_size); memmove(buf, buf + actual_size, FLATBUF_BUF_SIZE - actual_size); *buf_size -= actual_size; + // Clear so that no false positives memset(buf + *buf_size, 0, FLATBUF_BUF_SIZE - *buf_size); return GetDownlinkMsg(out_buf); @@ -421,6 +426,7 @@ void handle_acks(const uint8_t frame_id) { void json_log_to_serial(const DownlinkMsg *const msg) { const uint64_t tstamp = msg->TimeStamp(); + sendJsonInt( tstamp, GS_RSSI, radio.getRSSI()); sendJsonInt( tstamp, COMMS_RECD, msg->Bytes()); sendJsonInt( tstamp, TPC_STATE, (int)msg->State()); sendJsonInt( tstamp, FC_PWRD, (int)msg->FCPowered()); @@ -428,7 +434,7 @@ void json_log_to_serial(const DownlinkMsg *const msg) { sendJsonFloat( tstamp, TPC_BATV, BATUINT_TO_FLOAT(msg->BattVoltage())); if (msg->FCMsg()) { - const FCUpdateMsg *const fc = msg->FCMsg(); + const FCUpdateMsg *fc = msg->FCMsg(); sendJsonInt( tstamp, FC_STATE, (int)fc->State()); sendJsonFloat(tstamp, FC_ALT, fc->Altitude()); }