eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 3 Releases 117 Wiki Activity

command to request camera parameters

Browse Source
This commit is contained in:
Jakob Meier 2022-01-17 13:15:18 +01:00
parent 952dfde5b7
commit abfc120633
3 changed files with 181 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
bsp_q7s/devices/startracker/StarTrackerDefinitions.h
View File

@ -196,7 +196,22 @@ enum PoolIds: lp_id_t {
BLOBPIX_TOT_VAL,
BLOBPIX_IN_USE,
BLOBPIX_BRIGHT_NEIGHBOURS,
BLOBPIX_REGION
BLOBPIX_REGION,
CAMERA_MODE,
FOCALLENGTH,
EXPOSURE,
INTERVAL,
CAMERA_OFFSET,
PGAGAIN,
ADCGAIN,
CAM_REG1,
CAM_VAL1,
CAM_REG2,
CAM_VAL2,
CAM_REG3,
CAM_VAL3,
CAM_REG4,
CAM_VAL4
};
static const DeviceCommandId_t PING_REQUEST = 0;
@ -245,6 +260,7 @@ static const DeviceCommandId_t DOWNLOAD_FPGA_IMAGE = 63;
static const DeviceCommandId_t CHANGE_FPGA_DOWNLOAD_FILE = 64;
static const DeviceCommandId_t UPLOAD_FPGA_IMAGE = 65;
static const DeviceCommandId_t FPGA_ACTION = 66;
static const DeviceCommandId_t REQ_CAMERA_PARAMS = 67;
static const DeviceCommandId_t NONE = 0xFFFFFFFF;
static const uint32_t VERSION_SET_ID = REQ_VERSION;
@ -260,6 +276,7 @@ static const uint32_t DOWNLOADCENTROID_SET_ID = DOWNLOAD_CENTROID;
static const uint32_t DOWNLOAD_MATCHED_STAR_SET_ID = DOWNLOAD_MATCHED_STAR;
static const uint32_t DOWNLOAD_DBIMAGE_SET_ID = DOWNLOAD_DBIMAGE;
static const uint32_t DOWNLOAD_BLOBPIXEL_SET_ID = DOWNLOAD_BLOBPIXEL;
static const uint32_t CAMERA_SET_ID = REQ_CAMERA_PARAMS;
/** Max size of unencoded frame */
static const size_t MAX_FRAME_SIZE = 1200;
@ -277,6 +294,7 @@ static const uint8_t DOWNLOAD_CENTROID_SET_ENTRIES = 11;
static const uint8_t DOWNLOAD_MATCHED_STAR_SET_ENTRIES = 14;
static const uint8_t DOWNLOAD_DBIMAGE_SET_ENTRIES = 6;
static const uint8_t DOWNLOAD_BLOBPIXEL_SET_ENTRIES = 7;
static const uint8_t CAMERA_SET_ENTRIES = 15;
// Action, parameter and telemetry IDs
namespace ID {
@ -1206,5 +1224,70 @@ public:
sif::info << "DownloadBlobPixel::printSet: region: " << this->region << std::endl;
}
};
/**
* @brief Will store the camera parameters set in the star tracker which are retrieved with
* a get parameter request.
*/
class CameraSet: public StaticLocalDataSet<CAMERA_SET_ENTRIES> {
public:
// Size of dataset
static const size_t SIZE = 25;
CameraSet(HasLocalDataPoolIF* owner) :
StaticLocalDataSet(owner, CAMERA_SET_ID) {
}
CameraSet(object_id_t objectId) :
StaticLocalDataSet(sid_t(objectId, CAMERA_SET_ID)) {
}
lp_var_t<uint8_t> mode = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAMERA_MODE, this);
lp_var_t<float> focallength = lp_var_t<float>(sid.objectId, PoolIds::FOCALLENGTH, this);
lp_var_t<float> exposure = lp_var_t<float>(sid.objectId, PoolIds::EXPOSURE, this);
lp_var_t<float> interval = lp_var_t<float>(sid.objectId, PoolIds::INTERVAL, this);
lp_var_t<int16_t> offset = lp_var_t<int16_t>(sid.objectId, PoolIds::CAMERA_OFFSET, this);
lp_var_t<uint8_t> pgagain = lp_var_t<uint8_t>(sid.objectId, PoolIds::PGAGAIN, this);
lp_var_t<uint8_t> adcgain = lp_var_t<uint8_t>(sid.objectId, PoolIds::ADCGAIN, this);
lp_var_t<uint8_t> reg1 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG1, this);
lp_var_t<uint8_t> val1 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL1, this);
lp_var_t<uint8_t> reg2 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG2, this);
lp_var_t<uint8_t> val2 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL2, this);
lp_var_t<uint8_t> reg3 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG3, this);
lp_var_t<uint8_t> val3 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL3, this);
lp_var_t<uint8_t> reg4 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG4, this);
lp_var_t<uint8_t> val4 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL4, this);
void printSet() {
PoolReadGuard rg(this);
sif::info << "CameraSet::printSet: mode: " << static_cast<unsigned int>(this->mode.value)
<< std::endl;
sif::info << "CameraSet::printSet: focallength: " << this->focallength << std::endl;
sif::info << "CameraSet::printSet: exposure: " << this->exposure << std::endl;
sif::info << "CameraSet::printSet: interval: " << this->interval << std::endl;
sif::info << "CameraSet::printSet: offset: " << this->offset << std::endl;
sif::info << "CameraSet::printSet: PGA gain: "
<< static_cast<unsigned int>(this->pgagain.value) << std::endl;
sif::info << "CameraSet::printSet: ADC gain: "
<< static_cast<unsigned int>(this->adcgain.value) << std::endl;
sif::info << "CameraSet::printSet: Reg 1: "
<< static_cast<unsigned int>(this->reg1.value) << std::endl;
sif::info << "CameraSet::printSet: Val 1: "
<< static_cast<unsigned int>(this->val1.value) << std::endl;
sif::info << "CameraSet::printSet: Reg 2: "
<< static_cast<unsigned int>(this->reg2.value) << std::endl;
sif::info << "CameraSet::printSet: Val 2: "
<< static_cast<unsigned int>(this->val2.value) << std::endl;
sif::info << "CameraSet::printSet: Reg 3: "
<< static_cast<unsigned int>(this->reg3.value) << std::endl;
sif::info << "CameraSet::printSet: Val 3: "
<< static_cast<unsigned int>(this->val3.value) << std::endl;
sif::info << "CameraSet::printSet: Reg 4: "
<< static_cast<unsigned int>(this->reg4.value) << std::endl;
sif::info << "CameraSet::printSet: Val 4: "
<< static_cast<unsigned int>(this->val4.value) << std::endl;
}
};
}
#endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */

89
bsp_q7s/devices/startracker/StarTrackerHandler.cpp
View File

@ -8,6 +8,7 @@
extern "C" {
#include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
#include <thirdparty/arcsec_star_tracker/client/generated/actionreq.h>
#include <thirdparty/arcsec_star_tracker/client/generated/parameter.h>
#include "common/misc.h"
}
@ -18,7 +19,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
this), interfaceSet(this), timeSet(this), solutionSet(this), histogramSet(this), contrastSet(
this), checksumSet(this), downloadCentroidSet(this), downloadMatchedStar(this),
downloadDbImage(this), downloadBlobPixel(this), strHelper(
downloadDbImage(this), downloadBlobPixel(this), cameraSet(this), strHelper(
strHelper) {
if (comCookie == nullptr) {
sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
@ -495,6 +496,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
result = prepareFpgaActionCommand(commandData, commandDataLen);
return result;
}
case (StarTracker::REQ_CAMERA_PARAMS): {
result = prepareRequestCameraParams();
return result;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
@ -573,6 +578,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(StarTracker::FPGA_ACTION, 3, nullptr,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(StarTracker::REQ_CAMERA_PARAMS, 3, nullptr,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
}
ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
@ -604,7 +611,12 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema
}
case TMTC_SETPARAMREPLY: {
*foundLen = remainingSize - bytesLeft;
result = scanForParameterReply(foundId);
result = scanForSetParameterReply(foundId);
break;
}
case TMTC_PARAMREPLY: {
*foundLen = remainingSize - bytesLeft;
result = scanForGetParameterReply(foundId);
break;
}
case TMTC_TELEMETRYREPLYA:
@ -726,6 +738,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
result = handleSetParamReply();
break;
}
case (StarTracker::REQ_CAMERA_PARAMS): {
handleParamRequest(cameraSet, StarTracker::CameraSet::SIZE);
break;
}
default: {
sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
<< std::endl;
@ -934,6 +950,22 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
localDataPoolMap.emplace(StarTracker::BLOBPIX_IN_USE, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::BLOBPIX_BRIGHT_NEIGHBOURS, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::BLOBPIX_REGION, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAMERA_MODE, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::FOCALLENGTH, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(StarTracker::EXPOSURE, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(StarTracker::INTERVAL, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAMERA_OFFSET, new PoolEntry<int16_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::PGAGAIN, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::ADCGAIN, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_REG1, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_VAL1, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_REG2, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_VAL2, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_REG3, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_VAL3, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_REG4, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(StarTracker::CAM_VAL4, new PoolEntry<uint8_t>( { 0 }));
return RETURN_OK;
}
@ -1036,7 +1068,7 @@ ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId)
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::scanForParameterReply(DeviceCommandId_t *foundId) {
ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *foundId) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
case (StarTracker::ID::SUBSCRIBE): {
@ -1091,6 +1123,22 @@ ReturnValue_t StarTrackerHandler::scanForParameterReply(DeviceCommandId_t *found
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::scanForGetParameterReply(DeviceCommandId_t *foundId) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
case (StarTracker::ID::CAMERA): {
*foundId = StarTracker::REQ_CAMERA_PARAMS;
break;
}
default: {
sif::warning << "tarTrackerHandler::scanForGetParameterReply: UnkNown ID" << std::endl;
return RETURN_FAILED;
break;
}
}
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
@ -1700,6 +1748,16 @@ ReturnValue_t StarTrackerHandler::prepareFpgaActionCommand(const uint8_t* comman
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::prepareRequestCameraParams() {
struct Camera req;
uint32_t length = 0;
arc_pack_camera_parameter_req(commandBuffer, &length);
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::handleSetParamReply() {
const uint8_t* reply = dataLinkLayer.getReply();
uint8_t status = *(reply + STATUS_OFFSET);
@ -1788,6 +1846,31 @@ ReturnValue_t StarTrackerHandler::handleChecksumReply() {
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::handleParamRequest(LocalPoolDataSetBase& dataset, size_t size) {
ReturnValue_t result = RETURN_OK;
result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
return result;
}
const uint8_t* reply = dataLinkLayer.getReply() + PARAMS_OFFSET;
dataset.setValidityBufferGeneration(false);
result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
if (result != RETURN_OK) {
sif::warning << "StarTrackerHandler::handleParamRequest Deserialization failed"
<< std::endl;
}
dataset.setValidityBufferGeneration(true);
dataset.setValidity(true, true);
result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
return result;
}
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
dataset.printSet();
#endif
return result;
}
ReturnValue_t StarTrackerHandler::handlePingReply() {
ReturnValue_t result = RETURN_OK;
uint32_t pingId = 0;

12
bsp_q7s/devices/startracker/StarTrackerHandler.h
View File

@ -142,6 +142,7 @@ private:
// Max valid position value in upload image command
static const uint16_t MAX_POSITION= 4095;
static const uint8_t STATUS_OFFSET = 1;
static const uint8_t PARAMS_OFFSET = 1;
static const uint8_t TICKS_OFFSET = 2;
static const uint8_t TIME_OFFSET = 6;
static const uint8_t TM_DATA_FIELD_OFFSET = 14;
@ -260,6 +261,7 @@ private:
StarTracker::DownloadMatchedStar downloadMatchedStar;
StarTracker::DownloadDBImage downloadDbImage;
StarTracker::DownloadBlobPixel downloadBlobPixel;
StarTracker::CameraSet cameraSet;
// Pointer to object responsible for uploading and downloading images to/from the star tracker
StrHelper* strHelper = nullptr;
@ -318,7 +320,8 @@ private:
void slipInit();
ReturnValue_t scanForActionReply(DeviceCommandId_t *foundId);
ReturnValue_t scanForParameterReply(DeviceCommandId_t *foundId);
ReturnValue_t scanForSetParameterReply(DeviceCommandId_t *foundId);
ReturnValue_t scanForGetParameterReply(DeviceCommandId_t *foundId);
ReturnValue_t scanForTmReply(DeviceCommandId_t *foundId);
/**
@ -492,6 +495,11 @@ private:
*/
ReturnValue_t prepareFpgaActionCommand(const uint8_t* commandData, size_t commandDataLen);
/**
* @brief Will fill the command buffer with the command to request the set camera parameters.
*/
ReturnValue_t prepareRequestCameraParams();
/**
* @brief Handles action replies with datasets.
*/
@ -524,6 +532,8 @@ private:
ReturnValue_t handlePingReply();
ReturnValue_t handleParamRequest(LocalPoolDataSetBase& dataset, size_t size);
/**
* @brief Checks the loaded program by means of the version set
*/