From bd357e2804c2c54c8a81a4517cb0e32f5b1b3990 Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Fri, 12 Apr 2019 04:09:20 -0700 Subject: [PATCH 1/7] added ack for FC receiving commands from TPC sent by GS --- fc/fc.h | 54 ++++++++++++++++++++++++++++++++++--- general/msg_fc_update.fbs | 5 +++- tpc/tpc.h | 57 ++++++++++++++++++++++++++++++++++++--- 3 files changed, 107 insertions(+), 9 deletions(-) diff --git a/fc/fc.h b/fc/fc.h index ef5f4fb..12a440d 100644 --- a/fc/fc.h +++ b/fc/fc.h @@ -80,6 +80,9 @@ void loop() { } led_green = !bpIgnited[0]; led_blue = !bpIgnited[1]; + if (msg->AckReqd()) { + sendAck(msg->FrameID()); + } } } } @@ -124,7 +127,7 @@ void start() { alt.setModeAltimeter(); alt.setModeActive(); debug_uart.printf("altimiter whoami: 0x%X\r\n", alt.whoAmI()); - + buildCurrentMessage(); @@ -212,7 +215,8 @@ void buildCurrentMessage() { true, bpIgnited[3], false, bpIgnited[4], true, bpIgnited[5], - false, bpIgnited[6]); + false, bpIgnited[6], + FCUpdateType_StateUpdate, 0); builder.Finish(message); uint8_t bytes = (uint8_t)builder.GetSize(); @@ -223,13 +227,55 @@ void buildCurrentMessage() { // 0.0f, 1.0f, 2.0f, // 3.0f, 4.0f, 5.0f, // 6.0f, 7.0f, 8.0f, - last_alt, //10.0f, + 0.0f, //10.0f, false, bpIgnited[0], true, bpIgnited[1], false, bpIgnited[2], true, bpIgnited[3], false, bpIgnited[4], true, bpIgnited[5], - false, bpIgnited[6]); + 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 + FCState_Pad, + // 0.0f, 1.0f, 2.0f, + // 3.0f, 4.0f, 5.0f, + // 6.0f, 7.0f, 8.0f, + 0.0f, //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(); + ack = CreateFCUpdateMsg(builder, + bytes, // Fill in actual number of bytes + FCState_Pad, + // 0.0f, 1.0f, 2.0f, + // 3.0f, 4.0f, 5.0f, + // 6.0f, 7.0f, 8.0f, + 0.0f, //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()); +} diff --git a/general/msg_fc_update.fbs b/general/msg_fc_update.fbs index d06d192..728eb58 100644 --- a/general/msg_fc_update.fbs +++ b/general/msg_fc_update.fbs @@ -3,6 +3,7 @@ namespace Calstar; /* This is sent over radio as well so it should be kept small */ enum FCState : byte { Pad = 0, Flight = 1 /* add others to match the state machine in the FC firmware */ } +enum FCUpdateType : byte { StateUpdate = 0, Ack = 1 } table FCUpdateMsg { Bytes : uint8; State : FCState; @@ -31,6 +32,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..b0a5fbe 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) @@ -59,6 +64,10 @@ int remaining_send_size = 0; int remaining_send_buf_start = 0; us_timestamp_t last_radio_send_us; +// frame_id, +std::unordered_map, uint8_t>> + acks_remaining; + void start() { fcPower = 0; @@ -81,6 +90,8 @@ void start() { radio.setPowerDBm(20); debug_uart.printf("Radio init complete.\r\n"); + + t_last_resend = msgTimer.read_ms(); } void loop() { @@ -116,15 +127,29 @@ 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) { + 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("\r\n"); + } } } @@ -180,7 +205,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 +219,7 @@ void loop() { } } debug_uart.printf(")to FC.\r\n"); - rs422.write(uplinkMsgBuffer, msg->Bytes()); + sendUplinkMsgToFC(msg->Bytes(), msg->AckReqd()); } if (msg->AckReqd()) { sendAck(msg->FrameID()); @@ -414,4 +439,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){ + 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); + } + } +} From 12861272c627cd791061b195a646a244af9da60b Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Fri, 12 Apr 2019 15:49:48 -0700 Subject: [PATCH 2/7] tpc prints to debug when it receives ack --- tpc/tpc.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tpc/tpc.h b/tpc/tpc.h index b0a5fbe..a83d606 100644 --- a/tpc/tpc.h +++ b/tpc/tpc.h @@ -148,7 +148,7 @@ void loop() { if (acks_remaining.count(msg->FrameID()) == 1) { acks_remaining.erase(msg->FrameID()); } - debug_uart.printf("\r\n"); + debug_uart.printf("Received Ack for Message: %d\r\n", msg->FrameID()); } } } From 95501fa46a2b7c7e250efcb5f35e8e03d62335fb Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Thu, 18 Apr 2019 23:55:32 -0700 Subject: [PATCH 3/7] moved debug print to only print if message dequeued from ack requested messages --- tpc/tpc.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tpc/tpc.h b/tpc/tpc.h index a83d606..042ac80 100644 --- a/tpc/tpc.h +++ b/tpc/tpc.h @@ -147,8 +147,9 @@ void loop() { 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()); } - debug_uart.printf("Received Ack for Message: %d\r\n", msg->FrameID()); + } } } From d1a8f6690870ed103ebfdf75070c47547263e0b3 Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Fri, 19 Apr 2019 00:44:57 -0700 Subject: [PATCH 4/7] added some missing variable declarations in tpc.h --- tpc/tpc.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tpc/tpc.h b/tpc/tpc.h index 042ac80..f6d06c1 100644 --- a/tpc/tpc.h +++ b/tpc/tpc.h @@ -33,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; @@ -63,6 +65,7 @@ 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>> @@ -220,7 +223,7 @@ void loop() { } } debug_uart.printf(")to FC.\r\n"); - sendUplinkMsgToFC(msg->Bytes(), msg->AckReqd()); + sendUplinkMsgToFC(msg->Bytes(), msg->AckReqd(), msg->FrameID()); } if (msg->AckReqd()) { sendAck(msg->FrameID()); @@ -442,7 +445,7 @@ void sendAck(uint8_t frame_id) { radio.send(builder.GetBufferPointer(), builder.GetSize()); } -void sendUplinkMsgToFC(uint8_t numBytes, bool with_ack){ +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}}); From fe451adca6f42879de878cc9ae088d34423a03ec Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Fri, 19 Apr 2019 00:52:17 -0700 Subject: [PATCH 5/7] added missing variable declarations to fc.h --- fc/fc.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/fc/fc.h b/fc/fc.h index 3be8513..5265a30 100644 --- a/fc/fc.h +++ b/fc/fc.h @@ -72,6 +72,7 @@ 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); /* @@ -436,7 +437,7 @@ void sendAck(uint8_t frame_id) { const uint8_t bytes = (uint8_t)builder.GetSize(); builder.Reset(); - ack = CreateFCUpdateMsg(builder, + Offset ack = CreateFCUpdateMsg(builder, bytes, // Fill in actual number of bytes FCState_Pad, // 0.0f, 1.0f, 2.0f, From b69904fb4d8e0ebbe60b42475b77a9fee4b673bc Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Fri, 19 Apr 2019 03:58:16 -0700 Subject: [PATCH 6/7] fixed typo messages errors --- fc/fc.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/fc/fc.h b/fc/fc.h index 5265a30..54e8658 100644 --- a/fc/fc.h +++ b/fc/fc.h @@ -400,11 +400,11 @@ void buildCurrentMessage() { builder.Reset(); message = CreateFCUpdateMsg(builder, bytes, // Fill in actual number of bytes - FCState_Pad, + state, // 0.0f, 1.0f, 2.0f, // 3.0f, 4.0f, 5.0f, // 6.0f, 7.0f, 8.0f, - 0.0f, //10.0f, + state == FCState_Setup ? 0 : root->value, //10.0f, false, bpIgnited[0], true, bpIgnited[1], false, bpIgnited[2], @@ -420,11 +420,11 @@ void sendAck(uint8_t frame_id) { builder.Reset(); Offset message = CreateFCUpdateMsg(builder, 1, // Can't be 0 or it will be ignored - FCState_Pad, + state, // 0.0f, 1.0f, 2.0f, // 3.0f, 4.0f, 5.0f, // 6.0f, 7.0f, 8.0f, - 0.0f, //10.0f, + state == FCState_Setup ? 0 : root->value, //10.0f, false, bpIgnited[0], true, bpIgnited[1], false, bpIgnited[2], @@ -439,7 +439,7 @@ void sendAck(uint8_t frame_id) { builder.Reset(); Offset ack = CreateFCUpdateMsg(builder, bytes, // Fill in actual number of bytes - FCState_Pad, + state, // 0.0f, 1.0f, 2.0f, // 3.0f, 4.0f, 5.0f, // 6.0f, 7.0f, 8.0f, From 63a59eaee31dceae8692eff49a929e4cf999c884 Mon Sep 17 00:00:00 2001 From: Ryan-Tran Date: Fri, 19 Apr 2019 04:15:46 -0700 Subject: [PATCH 7/7] I GOT RID OF THE MSG NULL CHECK IM BIG DUM --- tpc/tpc.h | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/tpc/tpc.h b/tpc/tpc.h index f6d06c1..a8a3e09 100644 --- a/tpc/tpc.h +++ b/tpc/tpc.h @@ -142,17 +142,18 @@ void loop() { 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->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()); + 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()); + } } - } } }