diff --git a/fc/fc.h b/fc/fc.h index 377337f..54e8658 100644 --- a/fc/fc.h +++ b/fc/fc.h @@ -35,7 +35,7 @@ struct Node { static Node* createList(int length) { Node* first = new Node(nullptr, 0); - Node* last = first; + Node* last = first; for (int i = 0; i < length; i++) { last = new Node(last, 0); last->previous->next = last; @@ -72,9 +72,10 @@ us_timestamp_t last_msg_send_us; uint8_t rs422_read_buf[BUF_SIZE]; UARTSerial rs422(RS422_TX, RS422_RX, RS422_BAUDRATE); +void sendAck(uint8_t frame_id); -/* +/* * Sets LED based on current state */ void updated_leds() { @@ -142,6 +143,9 @@ void read_messages() { } debug_uart.printf("\r\n"); } + if (msg->AckReqd()) { + sendAck(msg->FrameID()); + } } } } @@ -389,29 +393,72 @@ void buildCurrentMessage() { state == FCState_Setup ? 0 : root->value, // 10.0f, false, bpIgnited[0], true, bpIgnited[1], false, bpIgnited[2], true, bpIgnited[3], false, bpIgnited[4], - true, bpIgnited[5], false, bpIgnited[6]); + true, bpIgnited[5], false, bpIgnited[6], FCUpdateType_StateUpdate, 0); builder.Finish(message); uint8_t bytes = (uint8_t)builder.GetSize(); builder.Reset(); - message = - CreateFCUpdateMsg(builder, - bytes, // Fill in actual number of bytes - state, - // 0.0f, 1.0f, 2.0f, - // 3.0f, 4.0f, 5.0f, - // 6.0f, 7.0f, 8.0f, - // If in setup, we haven't zeroed altitude yet, so just send 0 instead - state == FCState_Setup ? 0 : root->value, // 10.0f, - false, bpIgnited[0], true, bpIgnited[1], false, - bpIgnited[2], true, bpIgnited[3], false, bpIgnited[4], - true, bpIgnited[5], false, bpIgnited[6]); + message = CreateFCUpdateMsg(builder, + bytes, // Fill in actual number of bytes + state, + // 0.0f, 1.0f, 2.0f, + // 3.0f, 4.0f, 5.0f, + // 6.0f, 7.0f, 8.0f, + state == FCState_Setup ? 0 : root->value, //10.0f, + false, bpIgnited[0], + true, bpIgnited[1], + false, bpIgnited[2], + true, bpIgnited[3], + false, bpIgnited[4], + true, bpIgnited[5], + false, bpIgnited[6], + FCUpdateType_StateUpdate, 0); builder.Finish(message); } +void sendAck(uint8_t frame_id) { + builder.Reset(); + Offset message = CreateFCUpdateMsg(builder, + 1, // Can't be 0 or it will be ignored + state, + // 0.0f, 1.0f, 2.0f, + // 3.0f, 4.0f, 5.0f, + // 6.0f, 7.0f, 8.0f, + state == FCState_Setup ? 0 : root->value, //10.0f, + false, bpIgnited[0], + true, bpIgnited[1], + false, bpIgnited[2], + true, bpIgnited[3], + false, bpIgnited[4], + true, bpIgnited[5], + false, bpIgnited[6], + FCUpdateType_Ack, frame_id); + builder.Finish(message); + + const uint8_t bytes = (uint8_t)builder.GetSize(); + builder.Reset(); + Offset ack = CreateFCUpdateMsg(builder, + bytes, // Fill in actual number of bytes + state, + // 0.0f, 1.0f, 2.0f, + // 3.0f, 4.0f, 5.0f, + // 6.0f, 7.0f, 8.0f, + state == FCState_Setup ? 0 : root->value, //10.0f, + false, bpIgnited[0], + true, bpIgnited[1], + false, bpIgnited[2], + true, bpIgnited[3], + false, bpIgnited[4], + true, bpIgnited[5], + false, bpIgnited[6], + FCUpdateType_Ack, frame_id); + builder.Finish(ack); + + rs422.write(builder.GetBufferPointer(), builder.GetSize()); +} int main() { start(); while (1) { loop(); } -} \ No newline at end of file +} diff --git a/general/msg_fc_update.fbs b/general/msg_fc_update.fbs index b90b091..46f2b78 100644 --- a/general/msg_fc_update.fbs +++ b/general/msg_fc_update.fbs @@ -1,7 +1,7 @@ namespace Calstar; /* This is sent over radio as well so it should be kept small */ - +enum FCUpdateType : byte { StateUpdate = 0, Ack = 1 } enum FCState : byte { Setup = 0, Pad = 1, @@ -41,6 +41,8 @@ table FCUpdateMsg { BP6Ignited : bool; BP7Continuity : bool; BP7Ignited : bool; + Type : FCUpdateType; + FrameID : uint8; } -root_type FCUpdateMsg; \ No newline at end of file +root_type FCUpdateMsg; diff --git a/tpc/tpc.h b/tpc/tpc.h index 5d26f09..a8a3e09 100644 --- a/tpc/tpc.h +++ b/tpc/tpc.h @@ -6,6 +6,8 @@ #include "mbed.h" #include "pins.h" #include +#include +#include #define DEBUG_UART_BAUDRATE (115200) #define RS422_BAUDRATE (115200) @@ -16,6 +18,9 @@ // random 16 bytes that must be the same across all nodes #define ENCRYPT_KEY ("CALSTARENCRYPTKE") +#define ACK_CHECK_INTERVAL_MS (200) +#define MAX_NUM_RETRIES (50) + // analog in is 0-1 from gnd to VCC (3.3V) // so multiply by 3.3V first, then multiply by inverse of resistive divider // (92.2kOhm / 502.2kOhm) @@ -28,9 +33,11 @@ using namespace Calstar; const UplinkMsg *getUplinkMsg(char c); const FCUpdateMsg *getFCUpdateMsg(char c); void buildCurrentMessage(); - +void resend_msgs(); void sendAck(uint8_t frame_id); +void sendUplinkMsgToFC(uint8_t numBytes, bool with_ack, uint8_t frame_id); + Timer msgTimer; FlatBufferBuilder builder(BUF_SIZE); const FCUpdateMsg *fcLatestData = NULL; @@ -58,6 +65,11 @@ uint8_t remaining_send_buf[BUF_SIZE]; int remaining_send_size = 0; int remaining_send_buf_start = 0; us_timestamp_t last_radio_send_us; +int32_t t_last_resend; + +// frame_id, +std::unordered_map, uint8_t>> + acks_remaining; void start() { fcPower = 0; @@ -81,6 +93,8 @@ void start() { radio.setPowerDBm(20); debug_uart.printf("Radio init complete.\r\n"); + + t_last_resend = msgTimer.read_ms(); } void loop() { @@ -116,14 +130,30 @@ void loop() { last_radio_send_us = current_time; } } + + //Resend messages awaiting ACK + if (msgTimer.read_ms() - t_last_resend > ACK_CHECK_INTERVAL_MS) { + resend_msgs(); + t_last_resend = msgTimer.read_ms(); + } + // Always read messages while (rs422.readable()) { ssize_t num_read = rs422.read(rs422_read_buf, BUF_SIZE); for (int i = 0; i < num_read; i++) { const FCUpdateMsg *msg = getFCUpdateMsg(rs422_read_buf[i]); - if (msg) { - fcLatestData = msg; - debug_uart.printf("Read alt=%f ft\r\n", fcLatestData->Altitude()); + if(msg){ + if (msg->Type() == FCUpdateType_StateUpdate) { + fcLatestData = msg; + debug_uart.printf("Read alt=%f ft\r\n", fcLatestData->Altitude()); + } + //msg->Type() == FCUpdateType_Ack + else { + if (acks_remaining.count(msg->FrameID()) == 1) { + acks_remaining.erase(msg->FrameID()); + debug_uart.printf("Received Ack for Message: %d\r\n", msg->FrameID()); + } + } } } } @@ -180,7 +210,7 @@ void loop() { fcPower = 0; debug_uart.printf(" Turned off FC power.\r\n"); } else if (msg->Type() == UplinkType_Ack) { - // TODO: deal with acks + // TODO: deal with acks, if TPC ever needs to send a message that requires ACK debug_uart.printf(" Acking message receive (TODO).\r\n"); } else { // For other messages we just forward to FC. @@ -194,7 +224,7 @@ void loop() { } } debug_uart.printf(")to FC.\r\n"); - rs422.write(uplinkMsgBuffer, msg->Bytes()); + sendUplinkMsgToFC(msg->Bytes(), msg->AckReqd(), msg->FrameID()); } if (msg->AckReqd()) { sendAck(msg->FrameID()); @@ -414,4 +444,28 @@ void sendAck(uint8_t frame_id) { false, 0, DownlinkType_Ack); builder.Finish(ack); radio.send(builder.GetBufferPointer(), builder.GetSize()); -} \ No newline at end of file +} + +void sendUplinkMsgToFC(uint8_t numBytes, bool with_ack, uint8_t frame_id){ + if (with_ack) { + acks_remaining.insert( + {frame_id, {std::vector(uplinkMsgBuffer, uplinkMsgBuffer + numBytes), 0}}); + debug_uart.printf("1"); + } + rs422.write(uplinkMsgBuffer, numBytes); + debug_uart.printf("2"); +} + +void resend_msgs() { + for (auto &msg : acks_remaining) { + debug_uart.printf("![RESENDING FRAME '%d']!\r\n", (int)msg.first); + const std::vector &vec = std::get<0>(msg.second); + rs422.write(vec.data(), vec.size()); + // pc.printf("Complete\r\n"); + std::get<1>(msg.second) = std::get<1>(msg.second) + 1; + if (std::get<1>(msg.second) >= MAX_NUM_RETRIES) { + debug_uart.printf("![FAILED TO SEND FRAME '%d']!\r\n", msg.first); + acks_remaining.erase(msg.first); + } + } +}