eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

RwHandler first commands

Browse Source
This commit is contained in:
Jakob Meier
2021-06-21 09:50:26 +02:00
parent 0913442804
commit 33a55d7114
16 changed files with 750 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
mission/devices/CMakeLists.txt
View File

@ -16,6 +16,7 @@ target_sources(${TARGET_NAME} PUBLIC
PlocHandler.cpp
RadiationSensorHandler.cpp
GyroADIS16507Handler.cpp
RwHandler.cpp
)

259
mission/devices/RwHandler.cpp Normal file
View File

@ -0,0 +1,259 @@
#include "RwHandler.h"
#include "OBSWConfig.h"
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <math.h>
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), statusSet(this) {
if (comCookie == NULL) {
sif::error << "RwHandler: Invalid com cookie" << std::endl;
}
}
RwHandler::~RwHandler() {
}
void RwHandler::doStartUp() {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
#else
setMode(_MODE_TO_ON);
#endif
}
void RwHandler::doShutDown() {
}
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
switch (communicationStep) {
case CommunicationStep::READ_TEMPERATURE:
*id = RwDefinitions::GET_TEMPERATURE;
break;
case CommunicationStep::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
break;
default:
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
}
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
return RETURN_OK;
}
ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t * commandData, size_t commandDataLen) {
ReturnValue_t result = RETURN_OK;
switch (deviceCommand) {
case (RwDefinitions::GET_RW_STATUS): {
prepareGetStatusCmd(commandData, commandDataLen);
return RETURN_OK;
}
case (RwDefinitions::SET_SPEED): {
if (commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl;
return SET_SPEED_COMMAND_INVALID_LENGTH;
}
result = checkSpeedAndRampTime(commandData, commandDataLen);
if (result != RETURN_OK) {
return result;
}
prepareSetSpeedCmd(commandData, commandDataLen);
return result;
}
case (RwDefinitions::GET_TEMPERATURE): {
prepareGetTemperatureCmd();
return RETURN_OK;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_FAILED;
}
void RwHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet,
RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
RwDefinitions::SIZE_SET_SPEED_REPLY);
}
ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
switch (*(start + 1)) {
case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
*foundId = RwDefinitions::GET_RW_STATUS;
break;
}
case (static_cast<uint8_t>(RwDefinitions::SET_SPEED)): {
*foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
*foundId = RwDefinitions::SET_SPEED;
break;
}
case (static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE)): {
*foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
*foundId = RwDefinitions::GET_TEMPERATURE;
break;
}
default: {
sif::debug << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
return RETURN_FAILED;
break;
}
}
sizeOfReply = *foundLen;
return RETURN_OK;
}
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
/** Check result code */
if (*(packet + 1) != 0) {
return EXECUTION_FAILED;
}
/** Received in little endian byte order */
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2) ;
if (CRC::crc16ccitt(packet, sizeOfReply, 0xFFFF) != replyCrc) {
return CRC_ERROR;
}
switch (id) {
case (RwDefinitions::GET_RW_STATUS): {
handleGetRwStatusReply(packet);
break;
}
case (RwDefinitions::SET_SPEED):
// no reply data expected
break;
case (RwDefinitions::GET_TEMPERATURE): {
handleTemperatureReply(packet);
break;
}
default: {
sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return RETURN_OK;
}
void RwHandler::setNormalDatapoolEntriesInvalid() {
}
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
return 5000;
}
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));
return RETURN_OK;
}
void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 3;
}
ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
int32_t speed = *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8
| *(commandData + 3);
if (speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
return INVALID_SPEED;
}
uint16_t rampTime = *commandData << 8 | *(commandData + 1);
if (rampTime < 10 || rampTime > 10000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time"
<< std::endl;
return INVALID_RAMP_TIME;
}
return RETURN_OK;
}
void RwHandler::prepareGetTemperatureCmd() {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 3;
}
void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
/** Speed (0.1 RPM) */
commandBuffer[1] = *(commandData + 3);
commandBuffer[2] = *(commandData + 2);
commandBuffer[3] = *(commandData + 1);
commandBuffer[4] = *commandData;
/** Ramp time (ms) */
commandBuffer[5] = *(commandData + 1);
commandBuffer[6] = *commandData;
uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 9;
}
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
uint8_t offset = 0;
statusSet.currSpeed = *(packet + 3) << 24 | *(packet + 2) << 16 | *(packet + 1) << 1 | *packet;
offset += 4;
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 1 | *(packet + offset);
offset += 4;
statusSet.state = *(packet + offset);
offset += 1;
statusSet.clcMode = *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed
<< " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: "
<< statusSet.referenceSpeed << " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: State is: " << statusSet.state
<< " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: " << statusSet.clcMode
<< " * 0.1 RPM" << std::endl;
#endif
}
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
temperatureSet.temperatureCelcius = *(packet + 3) << 24 | *(packet + 2) << 16
| *(packet + 1) << 1 | *packet;
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
<< temperatureSet.temperatureCelcius << " °C" << std::endl;
#endif
}

125
mission/devices/RwHandler.h Normal file
View File

@ -0,0 +1,125 @@
#ifndef MISSION_DEVICES_RWHANDLER_H_
#define MISSION_DEVICES_RWHANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/devices/devicedefinitions/RwDefinitions.h>
#include <string.h>
/**
* @brief This is the device handler for the reaction wheel from nano avionics.
*
* @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
* Arbeitsdaten/08_Used%20Components/Nanoavionics_Reactionwheels&fileid=181622
*
* @note Values are transferred in little endian format.
*
* @author J. Meier
*/
class RwHandler: public DeviceHandlerBase {
public:
RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie);
virtual ~RwHandler();
/**
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
*/
void setModeNormal();
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);
//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call
static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1);
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E
static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2);
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid substitution combination
static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3);
//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E
static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4);
protected:
void doStartUp() override;
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override;
void fillCommandAndReplyMap() override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t * commandData,size_t commandDataLen) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) override;
void setNormalDatapoolEntriesInvalid() override;
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
private:
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER;
//! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in the range of [-65000; 1000] or [1000; 65000]
static const ReturnValue_t INVALID_SPEED = MAKE_RETURN_CODE(0xA0);
//! [EXPORT] : [COMMENT] Action Message with invalid ramp time was received.
static const ReturnValue_t INVALID_RAMP_TIME = MAKE_RETURN_CODE(0xA1);
//! [EXPORT] : [COMMENT] Received set speed command has invalid length. Should be 6.
static const ReturnValue_t SET_SPEED_COMMAND_INVALID_LENGTH = MAKE_RETURN_CODE(0xA2);
//! [EXPORT] : [COMMENT] Command execution failed
static const ReturnValue_t EXECUTION_FAILED = MAKE_RETURN_CODE(0xA3);
//! [EXPORT] : [COMMENT] Reaction wheel reply has invalid crc
static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(0xA4);
RwDefinitions::TemperatureSet temperatureSet;
RwDefinitions::StatusSet statusSet;
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
enum class CommunicationStep {
READ_TEMPERATURE,
GET_RW_SATUS
};
CommunicationStep communicationStep = CommunicationStep::READ_TEMPERATURE;
size_t sizeOfReply = 0;
/**
* @brief This function prepares the send buffer with the data to request the status of
* the reaction wheel.
*/
void prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen);
/**
* @brief This function checks if the receiced speed and ramp time to set are in a valid
* range.
* @return RETURN_OK if successful, otherwise error code.
*/
ReturnValue_t checkSpeedAndRampTime(const uint8_t * commandData, size_t commandDataLen);
/**
* @brief This function prepares the set speed command from the commandData received with
* an action message.
*/
void prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen);
/**
* @brief This function fills the commandBuffer with the data to request the temperature.
*/
void prepareGetTemperatureCmd();
/**
* @brief This function handles the reply of the get temperature command.
*
* @param packet Pointer to the reply data
*/
void handleTemperatureReply(const uint8_t* packet);
/**
* @brief This function fills the status set with the data from the get-status-reply.
*/
void handleGetRwStatusReply(const uint8_t* packet);
};
#endif /* MISSION_DEVICES_RWHANDLER_H_ */

6
mission/devices/devicedefinitions/Max31865Definitions.h
View File

@ -1,5 +1,5 @@
#ifndef MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_
#define MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/datapoollocal/LocalPoolVariable.h>
@ -54,5 +54,5 @@ public:
}
#endif /* MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_ */
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_ */

86
mission/devices/devicedefinitions/RwDefinitions.h Normal file
View File

@ -0,0 +1,86 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include "objects/systemObjectList.h"
namespace RwDefinitions {
enum PoolIds: lp_id_t {
TEMPERATURE_C,
CURR_SPEED,
REFERENCE_SPEED,
STATE,
CLC_MODE
};
static constexpr DeviceCommandId_t GET_RW_STATUS = 4;
static constexpr DeviceCommandId_t SET_SPEED = 6;
static constexpr DeviceCommandId_t GET_TEMPERATURE = 8;
static constexpr uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
static constexpr uint32_t STATUS_SET_ID = GET_RW_STATUS;
static constexpr size_t SIZE_GET_RW_STATUS = 14;
static constexpr size_t SIZE_SET_SPEED_REPLY = 4;
static constexpr size_t SIZE_GET_TEMPERATURE_REPLY = 8;
/** Max size when requesting telemetry */
static constexpr size_t SIZE_GET_TELEMETRY_REPLY = 83;
/** Set speed command has maximum size */
static constexpr size_t MAX_CMD_SIZE = 9;
static constexpr size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY;
static constexpr uint8_t TEMPERATURE_SET_ENTRIES = 1;
static constexpr uint8_t STATUS_SET_ENTRIES = 4;
/**
* @brief This dataset can be used to store the temperature of a reaction wheel.
*/
class TemperatureSet:
public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
public:
TemperatureSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, TEMPERATURE_SET_ID) {
}
TemperatureSet(object_id_t objectId):
StaticLocalDataSet(sid_t(objectId, TEMPERATURE_SET_ID)) {
}
lp_var_t<int32_t> temperatureCelcius = lp_var_t<int32_t>(sid.objectId,
PoolIds::TEMPERATURE_C, this);
};
/**
* @brief This dataset can be used to store the reaction wheel status.
*/
class StatusSet:
public StaticLocalDataSet<STATUS_SET_ENTRIES> {
public:
StatusSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, STATUS_SET_ID) {
}
StatusSet(object_id_t objectId):
StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {
}
lp_var_t<int32_t> currSpeed = lp_var_t<int32_t>(sid.objectId,
PoolIds::CURR_SPEED, this);
lp_var_t<int32_t> referenceSpeed = lp_var_t<int32_t>(sid.objectId,
PoolIds::REFERENCE_SPEED, this);
lp_var_t<uint8_t> state = lp_var_t<uint8_t>(sid.objectId,
PoolIds::STATE, this);
lp_var_t<uint8_t> clcMode = lp_var_t<uint8_t>(sid.objectId,
PoolIds::CLC_MODE, this);
};
}
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */