/**
* This functions handles an incoming ECSS packet.
* If the ECSS packet is part of a large data transfer consisting from
* several sequential ECSS packets, it handles them automatically.
*
* In other words, there is no need to explicitly check for a fragmented data
* transfer.
*
* @param payload the received payload
* @param payload_size the size of the payload
* @return SATR_OK if all went ok or appropriate error code
*/
SAT_returnState
rx_ecss (uint8_t *payload, const uint16_t payload_size)
{
SAT_returnState ret;
tc_tm_pkt *pkt;
pkt = get_pkt (payload_size);
if (!C_ASSERT(pkt != NULL)) {
return SATR_ERROR;
}
if (unpack_pkt (payload, pkt, payload_size) == SATR_OK) {
ret = route_pkt (pkt);
}
TC_TM_app_id dest = 0;
if(pkt->type == TC) {
dest = pkt->app_id;
}
else if(pkt->type == TM) {
dest = pkt->dest_id;
}
if(dest == SYSTEM_APP_ID) {
free_pkt(pkt);
}
return ret;
}
void free_frame(frame *Frame)
{
if (Frame)
{
free_pkt(Frame->Pkt);
free(Frame);
}
}
void PacketQueue::clear()
{
boost::lock_guard<boost::mutex> lock(mutex);
for (auto it = blocks.begin(); it != blocks.end(); ++it)
{
for (auto it2 = (*it)->begin(); it2 != (*it)->end(); ++it2)
{
free_pkt(*it2);
}
delete *it;
}
blocks.clear();
}
bool PacketQueue::pop_front()
{
boost::lock_guard<boost::mutex> lock(mutex);
if (blocks.empty())
{
return false;
}
else
{
free_pkt(blocks.front()->front());
blocks.front()->pop_front();
if (blocks.front()->empty())
{
delete blocks.front();
blocks.pop_front();
}
return true;
}
}
SAT_returnState tx_ecss(tc_tm_pkt *pkt) {
int32_t ret = 0;
uint16_t size = 0;
SAT_returnState res;
if(pkt == NULL){
comms_rf_stats_frame_transmitted(&comms_stats, 0, SATR_ERROR);
return SATR_ERROR;
}
res = pack_pkt(send_buf, pkt, &size);
if(res != SATR_OK){
comms_rf_stats_frame_transmitted(&comms_stats, 0, res);
return ret;
}
ret = send_payload(send_buf, (size_t)size, 0,COMMS_DEFAULT_TIMEOUT_MS);
if(ret < 1){
return SATR_ERROR;
}
free_pkt (pkt);
return SATR_OK;
}
bool PacketQueue::insert(const AVPacket& pkt_)
{
AVPacket* pkt = new AVPacket(pkt_);
boost::lock_guard<boost::mutex> lock(mutex);
size_ += pkt->size;
//找第一个大于等于dts的block，如果能找到，说明将插在中间；通常是找不到的，即插在末尾
auto it_block = std::lower_bound(blocks.begin(), blocks.end(), pkt->dts, PacketBlockLess());
if (it_block == blocks.end())
{
if (blocks.empty() || blocks.back()->size() == m_block_size)
{
//全空，或者末尾的block满了，开个新的block
blocks.push_back(new PacketBlock());
}
blocks.back()->push_back(pkt);
}
else
{
//如果pkt小于该block的首个pkt，说明pkt其实应该插在上个block的尾部；
if (pkt->dts < (*it_block)->front()->dts)
{
if (it_block == blocks.begin()) //本身就是第一个，则在这之前插入一个block
{
it_block = blocks.insert(it_block, new PacketBlock());
}
else
{
it_block --; //往前挪一个
if ((*it_block)->size() == m_block_size)
{
//如果满了，则在此之后插入个新的block
it_block ++;
it_block = blocks.insert(it_block, new PacketBlock());
}
}
(*it_block)->push_back(pkt);
}
else
{
//否则，说明pkt是在该block的中间。除非有dts相等的，否则需要将该block拆成两半，将pkt放在前面的block里
auto it_pkt = std::lower_bound((*it_block)->begin(), (*it_block)->end(), pkt->dts, AVPacketLess());
//dts相等，覆盖并返回false；
if (pkt->dts == (*it_pkt)->dts)
{
free_pkt(*it_pkt);
*it_pkt = pkt;
return false;
}
else
{
//此时，it_pkt > dts，将从it_pkt开始的放到后面的半个block里，然后把pkt放在前面的半个block里
PacketBlock* old_pb = *it_block;
PacketBlock* new_pb = new PacketBlock(it_pkt, old_pb->end());
it_block ++;
blocks.insert(it_block, new_pb);
old_pb->erase(it_pkt, old_pb->end());
old_pb->push_back(pkt);
}
}
}
return true;
}
SAT_returnState
route_pkt (tc_tm_pkt *pkt)
{
SAT_returnState res;
TC_TM_app_id id;
if ( !C_ASSERT(pkt != NULL && pkt->data != NULL)) {
free_pkt(pkt);
return SATR_ERROR;
}
if (!C_ASSERT(pkt->type == TC || pkt->type == TM)) {
free_pkt(pkt);
return SATR_ERROR;
}
if (!C_ASSERT(pkt->app_id < LAST_APP_ID && pkt->dest_id < LAST_APP_ID)) {
free_pkt(pkt);
return SATR_ERROR;
}
if (pkt->type == TC) {
id = pkt->app_id;
}
else if (pkt->type == TM) {
id = pkt->dest_id;
}
else {
return SATR_ERROR;
}
if (id == SYSTEM_APP_ID && pkt->ser_type == TC_HOUSEKEEPING_SERVICE
&& pkt->ser_subtype == TM_HK_PARAMETERS_REPORT
&& pkt->data[0] == WOD_REP) {
/*
* A new WOD arrived from the OBC. Store it and extract the information.
* The transmission of each WOD is handled by the COMMS dispatcher function
*/
SYSVIEW_PRINT("WOD from OBC");
store_wod_obc(pkt->data + 1, pkt->len - 1);
}
else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_HOUSEKEEPING_SERVICE
&& pkt->ser_subtype == TM_HK_PARAMETERS_REPORT
&& pkt->data[0] == EXT_WOD_REP) {
/*
* A new exWOD arrived from the OBC. Store it and extract the information.
* The transmission of each exWOD is handled by the COMMS dispatcher function
*/
SYSVIEW_PRINT("exWOD from OBC");
store_ex_wod_obc(pkt->data, pkt->len);
}
else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_HOUSEKEEPING_SERVICE) {
res = hk_app (pkt);
}
else if (id == SYSTEM_APP_ID
&& pkt->ser_type == TC_FUNCTION_MANAGEMENT_SERVICE) {
res = function_management_app (pkt);
}
else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_LARGE_DATA_SERVICE) {
res = large_data_app (pkt);
if (res == SATR_OK) {
free_pkt (pkt);
return SATR_OK;
}
}
else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_TEST_SERVICE) {
//C_ASSERT(pkt->ser_subtype == 1 || pkt->ser_subtype == 2 || pkt->ser_subtype == 9 || pkt->ser_subtype == 11 || pkt->ser_subtype == 12 || pkt->ser_subtype == 13) { free_pkt(pkt); return SATR_ERROR; }
res = test_app (pkt);
}
else if (id == EPS_APP_ID) {
queuePush (pkt, OBC_APP_ID);
}
else if (id == ADCS_APP_ID) {
queuePush (pkt, OBC_APP_ID);
}
else if (id == OBC_APP_ID) {
queuePush (pkt, OBC_APP_ID);
}
else if (id == IAC_APP_ID) {
queuePush (pkt, OBC_APP_ID);
}
else if (id == GND_APP_ID) {
if (pkt->len > MAX_PKT_DATA) {
large_data_downlinkTx_api (pkt);
}
else {
tx_ecss (pkt);
}
}
else if (id == DBG_APP_ID) {
queuePush (pkt, OBC_APP_ID);
} else {
free_pkt(pkt);
}
return SATR_OK;
}