Commit 4e1be5ee authored by Manolis Surligas's avatar Manolis Surligas

Add a TCP rigctl message command accepter block.

The purpose of this block is to grab rigctl compatible commands from the
gr-satnogs client software and use the set_freq command in order to
compensate the doppler shift effect.
parent 73e2c8d9
......@@ -27,5 +27,6 @@ install(FILES
satnogs_ax25_encoder_bf.xml
satnogs_ax25_decoder_b.xml
satnogs_udp_msg_source.xml
satnogs_debug_msg_source.xml DESTINATION share/gnuradio/grc/blocks
satnogs_debug_msg_source.xml
satnogs_tcp_rigctl_msg_source.xml DESTINATION share/gnuradio/grc/blocks
)
<?xml version="1.0"?>
<block>
<name>TCP rigctl Message Source</name>
<key>satnogs_tcp_rigctl_msg_source</key>
<category>satnogs</category>
<import>import satnogs</import>
<make>satnogs.tcp_rigctl_msg_source($addr, $port, $mtu)</make>
<param>
<name>IP Address</name>
<key>addr</key>
<value>"127.0.0.1"</value>
<type>string</type>
</param>
<param>
<name>Listen port</name>
<key>port</key>
<value>16886</value>
<type>int</type>
</param>
<param>
<name>MTU</name>
<key>mtu</key>
<value>1500</value>
<type>int</type>
</param>
<source>
<name>freq</name>
<type>message</type>
</source>
</block>
\ No newline at end of file
......@@ -39,5 +39,6 @@ install(FILES
ax25_decoder_b.h
udp_msg_source.h
debug_msg_source.h
tc_tm.h DESTINATION include/satnogs
tc_tm.h
tcp_rigctl_msg_source.h DESTINATION include/satnogs
)
......@@ -133,12 +133,12 @@ namespace gr
*/
typedef enum
{
R_OBC_PKT_ILLEGAL_APPID = 0, //!< R_OBC_PKT_ILLEGAL_APPID illegal application ID
R_OBC_PKT_ILLEGAL_APPID = 0, //!< R_OBC_PKT_ILLEGAL_APPID illegal application ID
R_OBC_PKT_INV_LEN = 1, //!< R_OBC_PKT_INV_LEN invalid length
R_OBC_PKT_INC_CRC = 2, //!< R_OBC_PKT_INC_CRC incorrect CRC
R_OBC_PKT_ILLEGAL_PKT_TP = 3, //!< R_OBC_PKT_ILLEGAL_PKT_TP
R_OBC_PKT_ILLEGAL_PKT_STP = 4, //!< R_OBC_PKT_ILLEGAL_PKT_STP
R_OBC_PKT_ILLEGAL_APP_DATA = 5,//!< R_OBC_PKT_ILLEGAL_APP_DATA
R_OBC_PKT_ILLEGAL_APP_DATA = 5, //!< R_OBC_PKT_ILLEGAL_APP_DATA
R_OBC_OK = 6, //!< R_OBC_OK All ok
R_OBC_ERROR = 7, //!< R_OBC_ERROR an error occured
R_OBC_EOT = 8, //!< R_OBC_EOT End-of-transfer
......@@ -174,33 +174,217 @@ namespace gr
uint16_t dest_id;
uint8_t *data; /* variable data, this should be fixed array */
uint8_t padding; /* x bits, padding for word alligment */
uint8_t padding; /* x bits, padding for word alligment */
uint16_t crc; /* CRC or checksum, mission specific*/
} tc_tm_pkt;
extern const uint8_t services_verification_TC_TM[MAX_SERVICES][MAX_SUBTYPES][2];
extern const uint8_t app_id_verification[MAX_APP_ID];
extern const uint8_t services_verification_OBC_TC[MAX_SERVICES][MAX_SUBTYPES];
/*
extern const uint8_t services_verification_TC_TM[MAX_SERVICES][MAX_SUBTYPES][2];
extern const uint8_t app_id_verification[MAX_APP_ID];
extern const uint8_t services_verification_OBC_TC[MAX_SERVICES][MAX_SUBTYPES];
extern OBC_ret_state_t
verification_pack_pkt_api (uint8_t *buf, tc_tm_pkt *pkt,
uint16_t *buf_pointer);
extern OBC_ret_state_t
hk_pack_pkt_api (uint8_t *buf, tc_tm_pkt *pkt, uint16_t *buf_pointer);
extern OBC_ret_state_t
verification_pack_pkt_api (uint8_t *buf, tc_tm_pkt *pkt,
uint16_t *buf_pointer);
extern OBC_ret_state_t
hk_pack_pkt_api (uint8_t *buf, tc_tm_pkt *pkt, uint16_t *buf_pointer);
*/
static inline uint8_t
ecss_tm_checksum (const uint8_t *data, uint16_t size)
{
uint8_t CRC = 0;
for (int i = 0; i <= size; i++) {
CRC = CRC ^ data[i];
}
return CRC;
}
/*Must check for endianess*/
static inline OBC_ret_state_t
ecss_tm_unpack_pkt (const uint8_t *buf, tc_tm_pkt *pkt, const uint16_t size)
{
union _cnv cnv;
uint8_t tmp_crc[2];
uint8_t ver, dfield_hdr, ccsds_sec_hdr, tc_pus;
tmp_crc[0] = buf[size - 1];
tmp_crc[1] = checkSum (buf, size - 2);
ver = buf[0] >> 5;
pkt->type = (buf[0] >> 4) & 0x01;
dfield_hdr = (buf[0] >> 3) & 0x01;
cnv.cnv8[0] = buf[1];
cnv.cnv8[1] = 0x07 & buf[0];
pkt->app_id = cnv.cnv16[0];
pkt->seq_flags = buf[2] >> 6;
cnv.cnv8[0] = buf[3];
cnv.cnv8[1] = buf[2] & 0x3F;
pkt->seq_count = cnv.cnv16[0];
cnv.cnv8[0] = buf[4];
cnv.cnv8[1] = buf[5];
pkt->len = cnv.cnv16[0];
ccsds_sec_hdr = buf[6] >> 7;
tc_pus = buf[6] >> 4;
pkt->ack = 0x04 & buf[6];
pkt->ser_type = buf[7];
pkt->ser_subtype = buf[8];
pkt->dest_id = buf[9];
if (app_id_verification[pkt->app_id] != 1) {
return R_OBC_PKT_ILLEGAL_APPID;
}
if (pkt->len != size - 7) {
return R_OBC_PKT_INV_LEN;
}
if (tmp_crc[0] != tmp_crc[1]) {
return R_OBC_PKT_INC_CRC;
}
if (services_verification_TC_TM[pkt->ser_type][pkt->ser_subtype][pkt->type]
!= 1) {
return R_OBC_PKT_ILLEGAL_PKT_TP;
}
if (ver != 0) {
return R_OBC_ERROR;
}
if (tc_pus != 1) {
return R_OBC_ERROR;
}
if (ccsds_sec_hdr != 0) {
return R_OBC_ERROR;
}
if (pkt->type != TC && pkt->type != TM) {
return R_OBC_ERROR;
}
if (dfield_hdr != 1) {
return R_OBC_ERROR;
}
uint8_t
checkSum (const uint8_t *data, uint16_t size);
if (pkt->ack != TC_ACK_NO || pkt->ack != TC_ACK_ACC
|| pkt->ack != TC_ACK_EXE_COMP) {
return R_OBC_ERROR;
}
for (int i = 0; i < pkt->len - 4; i++) {
pkt->data[i] = buf[10 + i];
}
return R_OBC_OK;
}
/**
* Packs a TC packet into a byte buffer
* @param buf buffer to store the data to be sent
* @param pkt the data to be stored in the buffer
* @param size size of the array
* @return appropriate error code or R_OBC_OK if all operation succeed
*/
static inline OBC_ret_state_t
ecss_tm_pack_pkt (uint8_t *buf, tc_tm_pkt *pkt, uint16_t *size)
{
union _cnv cnv;
uint8_t buf_pointer;
cnv.cnv16[0] = pkt->app_id;
buf[0] = ( ECSS_VER_NUMBER << 5 | pkt->type << 4
| ECSS_DATA_FIELD_HDR_FLG << 3 | cnv.cnv8[1]);
buf[1] = cnv.cnv8[0];
cnv.cnv16[0] = pkt->seq_count;
buf[2] = (pkt->seq_flags << 6 | cnv.cnv8[1]);
buf[3] = cnv.cnv8[0];
/* TYPE = 0 TM, TYPE = 1 TC*/
if (pkt->type == TM) {
buf[6] = ECSS_PUS_VER << 4;
}
else if (pkt->type == TC) {
buf[6] = ( ECSS_SEC_HDR_FIELD_FLG << 7 | ECSS_PUS_VER << 4 | pkt->ack);
}
else {
return R_OBC_ERROR;
}
buf[7] = pkt->ser_type;
buf[8] = pkt->ser_subtype;
buf[9] = pkt->dest_id; /*source or destination*/
buf_pointer = 10;
if (pkt->ser_type == TC_VERIFICATION_SERVICE) {
//cnv.cnv16[0] = tc_pkt_id;
//cnv.cnv16[1] = tc_pkt_seq_ctrl;
/*verification_pack_pkt_api (buf, pkt, &buf_pointer);*/
}
else if (pkt->ser_type == TC_HOUSEKEEPING_SERVICE) {
/*hk_pack_pkt_api (buf, pkt, &buf_pointer);*/
}
else if (pkt->ser_type == TC_FUNCTION_MANAGEMENT_SERVICE
&& pkt->ser_subtype == 1) {
buf[10] = pkt->data[0];
buf[11] = pkt->data[1];
buf[12] = pkt->data[2];
buf[13] = pkt->data[3];
buf[14] = pkt->data[4];
buf_pointer += 5;
}
else {
return R_OBC_ERROR;
}
/*check if this is correct*/
cnv.cnv16[0] = buf_pointer - 6;
buf[4] = cnv.cnv8[0];
buf[5] = cnv.cnv8[1];
buf[buf_pointer] = checkSum (buf, buf_pointer - 1);
*size = buf_pointer;
return R_OBC_OK;
}
static inline OBC_ret_state_t
ecss_tm_crt_pkt (tc_tm_pkt *pkt, uint16_t app_id, uint8_t type, uint8_t ack,
uint8_t ser_type, uint8_t ser_subtype, uint16_t dest_id)
{
OBC_ret_state_t
unpack_pkt (const uint8_t *buf, tc_tm_pkt *pkt, const uint16_t size);
pkt->type = type;
pkt->app_id = app_id;
pkt->dest_id = dest_id;
OBC_ret_state_t
pack_pkt (uint8_t *buf, tc_tm_pkt *pkt, uint16_t *size);
pkt->ser_type = ser_type;
pkt->ser_subtype = ser_subtype;
OBC_ret_state_t
crt_pkt (tc_tm_pkt *pkt, uint16_t app_id, uint8_t type, uint8_t ack,
uint8_t ser_type, uint8_t ser_subtype, uint16_t dest_id);
return R_OBC_OK;
}
} // namespace satnogs
} // namespace gr
......
/* -*- c++ -*- */
/*
* gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
*
* Copyright (C) 2016, Libre Space Foundation <http://librespacefoundation.org/>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef INCLUDED_SATNOGS_TCP_RIGCTL_MSG_SOURCE_H
#define INCLUDED_SATNOGS_TCP_RIGCTL_MSG_SOURCE_H
#include <satnogs/api.h>
#include <gnuradio/block.h>
namespace gr
{
namespace satnogs
{
/*!
* \brief Block that accepts TCP messages with rigctl commands. Depending
* the command contents this block produces an appropriate PMT message
* to control other blocks in the flowgraph
* \ingroup satnogs
*
*/
class SATNOGS_API tcp_rigctl_msg_source : virtual public gr::block
{
public:
typedef boost::shared_ptr<tcp_rigctl_msg_source> sptr;
/**
* Rigctl TCP command accepter
* @param addr the address of the interface to listen at
* @param port the TCP port to listen for TCP connections
* @param mtu the maximum MTU
* @return
*/
static sptr
make (const std::string& addr, uint16_t port, size_t mtu = 1500);
};
} // namespace satnogs
} // namespace gr
#endif /* INCLUDED_SATNOGS_TCP_RIGCTL_MSG_SOURCE_H */
......@@ -37,7 +37,7 @@ list(APPEND satnogs_sources
ax25_decoder_b_impl.cc
udp_msg_source_impl.cc
debug_msg_source_impl.cc
tc_tm.cc )
tcp_rigctl_msg_source_impl.cc )
set(satnogs_sources "${satnogs_sources}" PARENT_SCOPE)
if(NOT satnogs_sources)
......
/* -*- c++ -*- */
/*
* gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
*
* Copyright (C) 2016, Libre Space Foundation <http://librespacefoundation.org/>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gnuradio/io_signature.h>
#include <satnogs/tc_tm.h>
namespace gr
{
namespace satnogs
{
uint8_t
checkSum (const uint8_t *data, uint16_t size)
{
uint8_t CRC = 0;
for (int i = 0; i <= size; i++) {
CRC = CRC ^ data[i];
}
return CRC;
}
/*Must check for endianess*/
OBC_ret_state_t
unpack_pkt (const uint8_t *buf, tc_tm_pkt *pkt, const uint16_t size)
{
union _cnv cnv;
uint8_t tmp_crc[2];
uint8_t ver, dfield_hdr, ccsds_sec_hdr, tc_pus;
tmp_crc[0] = buf[size - 1];
tmp_crc[1] = checkSum (buf, size - 2);
ver = buf[0] >> 5;
pkt->type = (buf[0] >> 4) & 0x01;
dfield_hdr = (buf[0] >> 3) & 0x01;
cnv.cnv8[0] = buf[1];
cnv.cnv8[1] = 0x07 & buf[0];
pkt->app_id = cnv.cnv16[0];
pkt->seq_flags = buf[2] >> 6;
cnv.cnv8[0] = buf[3];
cnv.cnv8[1] = buf[2] & 0x3F;
pkt->seq_count = cnv.cnv16[0];
cnv.cnv8[0] = buf[4];
cnv.cnv8[1] = buf[5];
pkt->len = cnv.cnv16[0];
ccsds_sec_hdr = buf[6] >> 7;
tc_pus = buf[6] >> 4;
pkt->ack = 0x04 & buf[6];
pkt->ser_type = buf[7];
pkt->ser_subtype = buf[8];
pkt->dest_id = buf[9];
if (app_id_verification[pkt->app_id] != 1) {
return R_OBC_PKT_ILLEGAL_APPID;
}
if (pkt->len != size - 7) {
return R_OBC_PKT_INV_LEN;
}
if (tmp_crc[0] != tmp_crc[1]) {
return R_OBC_PKT_INC_CRC;
}
if (services_verification_TC_TM[pkt->ser_type][pkt->ser_subtype][pkt->type]
!= 1) {
return R_OBC_PKT_ILLEGAL_PKT_TP;
}
if (ver != 0) {
return R_OBC_ERROR;
}
if (tc_pus != 1) {
return R_OBC_ERROR;
}
if (ccsds_sec_hdr != 0) {
return R_OBC_ERROR;
}
if (pkt->type != TC && pkt->type != TM) {
return R_OBC_ERROR;
}
if (dfield_hdr != 1) {
return R_OBC_ERROR;
}
if (pkt->ack != TC_ACK_NO || pkt->ack != TC_ACK_ACC
|| pkt->ack != TC_ACK_EXE_COMP) {
return R_OBC_ERROR;
}
for (int i = 0; i < pkt->len - 4; i++) {
pkt->data[i] = buf[10 + i];
}
return R_OBC_OK;
}
/**
* Packs a TC packet into a byte buffer
* @param buf buffer to store the data to be sent
* @param pkt the data to be stored in the buffer
* @param size size of the array
* @return appropriate error code or R_OBC_OK if all operation succeed
*/
OBC_ret_state_t
pack_pkt (uint8_t *buf, tc_tm_pkt *pkt, uint16_t *size)
{
union _cnv cnv;
uint8_t buf_pointer;
cnv.cnv16[0] = pkt->app_id;
buf[0] = ( ECSS_VER_NUMBER << 5 | pkt->type << 4
| ECSS_DATA_FIELD_HDR_FLG << 3 | cnv.cnv8[1]);
buf[1] = cnv.cnv8[0];
cnv.cnv16[0] = pkt->seq_count;
buf[2] = (pkt->seq_flags << 6 | cnv.cnv8[1]);
buf[3] = cnv.cnv8[0];
/* TYPE = 0 TM, TYPE = 1 TC*/
if (pkt->type == TM) {
buf[6] = ECSS_PUS_VER << 4;
}
else if (pkt->type == TC) {
buf[6] = ( ECSS_SEC_HDR_FIELD_FLG << 7 | ECSS_PUS_VER << 4 | pkt->ack);
}
else {
return R_OBC_ERROR;
}
buf[7] = pkt->ser_type;
buf[8] = pkt->ser_subtype;
buf[9] = pkt->dest_id; /*source or destination*/
buf_pointer = 10;
if (pkt->ser_type == TC_VERIFICATION_SERVICE) {
//cnv.cnv16[0] = tc_pkt_id;
//cnv.cnv16[1] = tc_pkt_seq_ctrl;
/*verification_pack_pkt_api (buf, pkt, &buf_pointer);*/
}
else if (pkt->ser_type == TC_HOUSEKEEPING_SERVICE) {
/*hk_pack_pkt_api (buf, pkt, &buf_pointer);*/
}
else if (pkt->ser_type == TC_FUNCTION_MANAGEMENT_SERVICE
&& pkt->ser_subtype == 1) {
buf[10] = pkt->data[0];
buf[11] = pkt->data[1];
buf[12] = pkt->data[2];
buf[13] = pkt->data[3];
buf[14] = pkt->data[4];
buf_pointer += 5;
}
else {
return R_OBC_ERROR;
}
/*check if this is correct*/
cnv.cnv16[0] = buf_pointer - 6;
buf[4] = cnv.cnv8[0];
buf[5] = cnv.cnv8[1];
buf[buf_pointer] = checkSum (buf, buf_pointer - 1);
*size = buf_pointer;
return R_OBC_OK;
}
OBC_ret_state_t
crt_pkt (tc_tm_pkt *pkt, uint16_t app_id, uint8_t type, uint8_t ack,
uint8_t ser_type, uint8_t ser_subtype, uint16_t dest_id)
{
pkt->type = type;
pkt->app_id = app_id;
pkt->dest_id = dest_id;
pkt->ser_type = ser_type;
pkt->ser_subtype = ser_subtype;
return R_OBC_OK;
}
} /* namespace satnogs */
} /* namespace gr */
/* -*- c++ -*- */
/*
* gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
*
* Copyright (C) 2016, Libre Space Foundation <http://librespacefoundation.org/>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gnuradio/io_signature.h>
#include "tcp_rigctl_msg_source_impl.h"
#include <satnogs/log.h>
#include <ifaddrs.h>
#include <errno.h>
#include <limits.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
namespace gr
{
namespace satnogs
{
tcp_rigctl_msg_source::sptr
tcp_rigctl_msg_source::make (const std::string& addr, uint16_t port,
size_t mtu)
{
return gnuradio::get_initial_sptr (
new tcp_rigctl_msg_source_impl (addr, port, mtu));
}
/*
* The private constructor
*/
tcp_rigctl_msg_source_impl::tcp_rigctl_msg_source_impl (
const std::string& addr, uint16_t port, size_t mtu) :
gr::block ("tcp_rigctl_msg_source",
gr::io_signature::make (0, 0, 0),
gr::io_signature::make (0, 0, 0)),
d_iface_addr (addr),
d_port (port),
d_mtu (mtu),
d_running (true)
{
message_port_register_out (pmt::mp ("freq"));
boost::shared_ptr<boost::thread> (
new boost::thread (
boost::bind (&tcp_rigctl_msg_source_impl::tcp_msg_accepter,
this)));
}
void
tcp_rigctl_msg_source_impl::tcp_msg_accepter ()
{
int sock;
int listen_sock;
struct sockaddr_in sin;
struct sockaddr client_addr;
socklen_t client_addr_len;
ssize_t ret;
uint8_t *buf;
double freq;
if ((listen_sock = socket (AF_INET, SOCK_STREAM, IPPROTO_TCP)) == -1) {
perror ("opening UDP socket");
exit (EXIT_FAILURE);
}
memset (&client_addr, 0, sizeof(struct sockaddr));
memset (&sin, 0, sizeof(struct sockaddr_in));
sin.sin_family = AF_INET;
sin.sin_port = htons (d_port);
sin.sin_addr.s_addr = INADDR_ANY;
if (inet_aton (d_iface_addr.c_str (), &(sin.sin_addr)) == 0) {
LOG_ERROR("Wrong IP address");
close (listen_sock);
exit (EXIT_FAILURE);
}
if (bind (listen_sock, (struct sockaddr *) &sin,
sizeof(struct sockaddr_in)) == -1) {
perror ("TCP bind");
close (listen_sock);
exit (EXIT_FAILURE);
}
if (listen (listen_sock, 1000) == -1) {
perror ("TCP listen");
close (listen_sock);
exit (EXIT_FAILURE);
}
/* All good until now. Allocate buffer memory and proceed */
buf = new uint8_t (d_mtu);
while (d_running) {
sock = accept (listen_sock, &client_addr, &client_addr_len);
if (sock <= 0) {
perror ("TCP accept");
exit (EXIT_FAILURE);
}
while ((ret = recv (sock, buf, d_mtu, 0)) > 0) {
switch (buf[0])
{
case 'F':
freq = get_freq_from_buf (buf + 2);