N2MStandardPacket::N2MStandardPacket()
{
std::lock_guard<std::mutex> packet_lock(packet_mtx);
n2m_standard_packet.header = 0;
n2m_standard_packet.vel_x = 0;
n2m_standard_packet.vel_y = 0;
n2m_standard_packet.pos_z = 0;
n2m_standard_packet.rot_x = 0;
n2m_standard_packet.rot_y = 0;
n2m_standard_packet.rot_z = 0;
n2m_standard_packet.mode = 0;
}
double N2MStandardPacket::get_rot_z()
{
std::lock_guard<std::mutex> packet_lock(packet_mtx);
return ((double)n2m_standard_packet.rot_z) * pow(2, -Q);
}
void N2MStandardPacket::set_rot_z(double rot_z)
{
int32_t rot_z_q = (int32_t)round(rot_z * pow(2, Q));
std::lock_guard<std::mutex> packet_lock(packet_mtx);
n2m_standard_packet.rot_z = rot_z_q;
}
void N2MStandardPacket::set_pos_z(double pos_z)
{
uint32_t pos_z_q = (uint32_t)round(pos_z * pow(2, Q));
std::lock_guard<std::mutex> packet_lock(packet_mtx);
n2m_standard_packet.pos_z = pos_z_q;
}
uint32_t N2MStandardPacket::get_header()
{
std::lock_guard<std::mutex> packet_lock(packet_mtx);
return n2m_standard_packet.header;
}
void N2MStandardPacket::set_vel_y(double vel_y)
{
int32_t vel_y_q = (int32_t)round(vel_y * pow(2, Q));
std::lock_guard<std::mutex> packet_lock(packet_mtx);
n2m_standard_packet.vel_y = vel_y_q;
}
void N2MStandardPacket::get_buffer(unsigned char buffer[])
{
std::lock_guard<std::mutex> packet_lock(packet_mtx);
memcpy(buffer, &n2m_standard_packet, n2m_standard_packet_size);
}
std::bitset<32> N2MStandardPacket::get_mode()
{
std::lock_guard<std::mutex> packet_lock(packet_mtx);
return std::bitset<32>(n2m_standard_packet.mode);
}
void N2MStandardPacket::set_mode(std::bitset<32> mode)
{
std::lock_guard<std::mutex> packet_lock(packet_mtx);
n2m_standard_packet.mode = mode.to_ulong();
}
inline void parseJsonPacket(packet_t *jsonPacket) {
json_t *root;
json_error_t jsonError;
char *errorString;
strbuffer_t *errorBuffer;
strbuffer_t *logMsg;
if(!jsonPacket || (jsonPacket->type != PKG_TYPE_INCOME_MESSAGE_STRING)) {
logMsg = strbuffer_new();
strbuffer_append(logMsg, "parseJsonPacket : Wrong packet type was received. Got ");
strbuffer_append(logMsg, int_to_string(jsonPacket->type));
strbuffer_append(logMsg, " instead of ");
strbuffer_append(logMsg, int_to_string(PKG_TYPE_INCOME_MESSAGE_STRING));
logger(LEVEL_ERR, logMsg->value);
strbuffer_destroy(&logMsg);
return;
}
strbuffer_t *jsonString = jsonPacket->payload.stringData;
if(!jsonString) {
char *errorMessage = "parseJsonPacket : Unable to extract json string from packet";
logger(LEVEL_ERR, errorMessage);
errorString = format_jsonrpc_error(JSONRPC_SERVER_ERROR, MSG_JSONRPC_ERRORS.server_error, errorMessage, 0);
errorBuffer = strbuffer_new();
strbuffer_append(errorBuffer, errorString);
packet_lock(jsonPacket);
{
jsonPacket->type = PKG_TYPE_OUTGOING_MESSAGE_STRING;
jsonPacket->payload.stringData = errorBuffer;
}
packet_unlock(jsonPacket);
return;
}
logMsg = strbuffer_new();
strbuffer_append(logMsg, "parseJsonPacket Received json packet: transport=");
strbuffer_append(logMsg, int_to_string(jsonPacket->transport));
strbuffer_append(logMsg, " length=");
strbuffer_append(logMsg, int_to_string(jsonString->length));
logger(LEVEL_INFO, logMsg->value);
strbuffer_destroy(&logMsg);
root = json_loads(jsonString->value, 0, &jsonError);
//packet_destroy(jsonPacket);
if(root != NULL) {
/* TODO decode array or object
array --> batch (optional, but will be implemented only if there will be enough resources mcu flash)
if batch proceed requests one by one and put the batch flag
*/
logger(LEVEL_DEBUG, "parseJsonPacket String parsing was successful");
if(isJsonRPCVersion2_0(root)){
logMsg = strbuffer_new();
strbuffer_append(logMsg, "parseJsonPacket Packet parsing succeed. Transport=");
strbuffer_append(logMsg, int_to_string(jsonPacket->transport));
logger(LEVEL_INFO, logMsg->value);
strbuffer_destroy(&logMsg);
json_t* idObj = json_object_get(root, "id");
json_int_t id = json_integer_value(idObj);
packet_lock(jsonPacket);
{
strbuffer_destroy(&(jsonPacket->payload.stringData));
jsonPacket->type = PKG_TYPE_INCOME_JSONRPC_REQUEST;
jsonPacket->payload.jsonDoc = root;
jsonPacket->id = id;
}
packet_unlock(jsonPacket);
return;
} else {
// TODO print_help(transport);
logMsg = strbuffer_new();
strbuffer_append(logMsg, ".parseJsonPacket : ");
strbuffer_append(logMsg, MSG_MAINTASKS.parseJsonPacket.invalid_jsonrpc_2_0);
logger(LEVEL_WARN, logMsg->value);
strbuffer_destroy(&logMsg);
json_int_t id = 0;
if(json_object_get(root, "id") != NULL) {
id = json_integer_value(json_object_get(root, "id"));
}
json_decref(root);
errorString = format_jsonrpc_error(JSONRPC_INVALID_REQUEST, MSG_JSONRPC_ERRORS.invalid_request, MSG_MAINTASKS.parseJsonPacket.invalid_jsonrpc_2_0, id);
errorBuffer = strbuffer_new();
strbuffer_append(errorBuffer, errorString);
packet_lock(jsonPacket);
{
strbuffer_destroy(&(jsonPacket->payload.stringData));
jsonPacket->type = PKG_TYPE_OUTGOING_MESSAGE_STRING;
jsonPacket->payload.stringData = errorBuffer;
jsonPacket->id = id;
}
packet_unlock(jsonPacket);
return;
}
} else {
strbuffer_t *errorText = strbuffer_new();
format_error_text(errorText, &jsonError);
logMsg = strbuffer_new();
strbuffer_append(logMsg, "parseJsonPacket Packet parsing failed: ");
strbuffer_append(logMsg, errorText->value);
logger(LEVEL_WARN, logMsg->value);
strbuffer_destroy(&logMsg);
errorString = format_jsonrpc_error(JSONRPC_PARSE_ERROR, MSG_JSONRPC_ERRORS.parse_error, errorText->value, 0);
errorBuffer = strbuffer_new();
strbuffer_append(errorBuffer, errorString);
strbuffer_destroy(&errorText);
packet_lock(jsonPacket);
{
strbuffer_destroy(&(jsonPacket->payload.stringData));
jsonPacket->type = PKG_TYPE_OUTGOING_MESSAGE_STRING;
jsonPacket->payload.stringData = errorBuffer;
}
packet_unlock(jsonPacket);
return;
}
}
void tskHandleRequests(void *pvParameters) {
packet_t *requestPacket = NULL;
packet_t *responsePacket = NULL;
json_t *idObj;
json_int_t id;
char *errorString;
strbuffer_t *errorBuffer;
strbuffer_t *logMsg;
portBASE_TYPE xStatus;
while(1) {
if(uxQueueMessagesWaiting(requestQueue) > 0) {
xStatus = xQueueReceive( requestQueue, &requestPacket, (portTickType) QUEUE_RECEIVE_WAIT_TIMEOUT );
if( (xStatus == pdPASS) && requestPacket) {
if(check_packet_type(requestPacket, PKG_TYPE_INCOME_JSONRPC_REQUEST) != TRUE) {
logger(LEVEL_ERR, "Wrong data packet from requestQueue. Deleting packet" );
packet_destroy(&requestPacket);
continue;
}
logMsg = strbuffer_new();
strbuffer_append(logMsg, "Received new request: id=");
strbuffer_append(logMsg, int_to_string(requestPacket->id));
strbuffer_append(logMsg, " transport=");
strbuffer_append(logMsg, int_to_string(requestPacket->transport));
logger(LEVEL_INFO, logMsg->value);
strbuffer_destroy(&logMsg);
handle_request(requestPacket);
responsePacket = requestPacket;
if(responsePacket) {
if(requestPacket->type == PKG_TYPE_OUTCOME_JSONRPC_NOTIFICATION) {
logger(LEVEL_INFO, "Request was notification only. Destroying response");
packet_destroy(&requestPacket);
packet_destroy(&responsePacket);
continue;
}
if(check_packet_type(requestPacket, PKG_TYPE_OUTCOME_JSONRPC_RESPONSE) != TRUE) {
logger(LEVEL_ERR, "Wrong response packet. Deleting packet" );
packet_destroy(&requestPacket);
continue;
}
logMsg = strbuffer_new();
strbuffer_append(logMsg, "Received response: id=");
strbuffer_append(logMsg, int_to_string(responsePacket->id));
strbuffer_append(logMsg, " transport=");
strbuffer_append(logMsg, int_to_string(responsePacket->transport));
logger(LEVEL_INFO, logMsg->value);
strbuffer_destroy(&logMsg);
logger(LEVEL_DEBUG, "creating json string from json object");
strbuffer_t *stringData = strbuffer_new();
char *jsonData = json_dumps(responsePacket->payload.jsonDoc, JSON_ENCODE_ANY );
if(jsonData) {
logger(LEVEL_DEBUG, "json string was created. Creating packet to send.");
strbuffer_append(stringData, jsonData);
logMsg = strbuffer_new();
strbuffer_append(logMsg, "Sending new packet: packet_length=");
strbuffer_append(logMsg, int_to_string(stringData->length));
strbuffer_append(logMsg, " packet_transport=");
strbuffer_append(logMsg, int_to_string(responsePacket->transport));
logger(LEVEL_DEBUG, logMsg->value);
strbuffer_destroy(&logMsg);
} else {
logMsg = strbuffer_new();
strbuffer_append(logMsg, "Unable to create string from response json: id=");
strbuffer_append(logMsg, int_to_string(responsePacket->id));
strbuffer_append(logMsg, " transport=");
strbuffer_append(logMsg, int_to_string(responsePacket->transport));
logger(LEVEL_WARN, logMsg->value);
strbuffer_destroy(&logMsg);
errorString = format_jsonrpc_error(JSONRPC_SERVER_ERROR, MSG_JSONRPC_ERRORS.server_error, "internal server error", responsePacket->id);
strbuffer_append(stringData, errorString);
}
vPortFree(jsonData);
packet_lock(responsePacket);
{
json_decref(responsePacket->payload.jsonDoc);
responsePacket->type = PKG_TYPE_OUTGOING_MESSAGE_STRING;
responsePacket->payload.stringData = stringData;
}
packet_unlock(responsePacket);
sendOutputMessage(responsePacket);
// idObj = json_object_get(responseJson, "id");
// id = json_integer_value(idObj);
//
// json_t *transportObject = json_object_get(responseJson, "transport");
// json_int_t transport = json_integer_value(transportObject);
//
// logger_format(LEVEL_INFO, "Received response. id = %d transport=%s", (int) id, transport_type_to_str((transport_type_t) transport));
//
// xStatus = xQueueSendToBack( responseQueue, &responseJson, (portTickType) QUEUE_SEND_WAIT_TIMEOUT );
// if( xStatus != pdPASS ){
// char* idStr = json_dumps(json_object_get(requestPacket, "id"), JSON_ENCODE_ANY);
// json_int_t transport = json_integer_value(json_object_get(requestPacket, "transport"));
// json_object_del(requestPacket, "transport");
// json_decref(requestJson);
// // send using error reporting function
// if(id && transport) {
// report_error_to_sender(
// (transport_type_t) transport,
// MSG_JSONRPC_ERRORS.general_error_json,
// JSONRPC_SERVER_ERROR, /* <-- code */
// MSG_JSONRPC_ERRORS.server_error, /* <-- message */
// MSG_MAINTASKS.tskAbstractReader.device_is_busy_timeout , /* <-- data */
// idStr
// );
// }
// if(idStr) vPortFree(idStr);
// logger_format(LEVEL_INFO, "Unable to add response to queue. Request id = %d", (int) id );
// } else {
// logger(LEVEL_DEBUG, "Response was added to response queue");
// }
}
// json_decref(requestPacket);
}
}
taskYIELD();
}
}
