void CTDriver::CTPort::performCmd(byte target,ByteVec cmd,ByteVec &resp,
std::ostream *mLogger,bool consumeStatus ) {
byte response[512];
ushort lenr = sizeof(response);
byte sad = CTSAD_HOST;
dump(mLogger,string("CTAPI->cmd ") + (target == CTDAD_CT ? "CT " : "ICC " ),cmd);
byte res = dri->pCTData(mCtn,&target,&sad,
ushort(cmd.size()),&cmd[0],&lenr,response);
if (res!=CTERR_OK || lenr < 2)
throw CTAPIError("performCmd",res,lenr,0,0);
resp.resize(0);
resp.insert(resp.end(),response,response+lenr);
dump(mLogger,"CTAPI<-resp ",resp);
if (consumeStatus) {
byte SW1 = resp[ lenr - 2 ];
byte SW2 = resp[ lenr - 1 ];
if (SW1 != 0x90) {
if (sad == CTDAD_CT) {
if (SW1==0x64 && ( SW2 == 0x00 || SW2 == 0x01 || SW2 == 0x02) ) {
throw AuthError(SW1,SW2);
}
}
throw CardError(SW1,SW2);
}
resp.resize(lenr - 2);
}
}
ByteVec read_file(const char* path) throw(std::runtime_error)
{
const size_t CHUNK_SIZE = 32 * 1024; // 32kB ~= 1-2 kloc
FILE* fp = fopen(path, "rb");
if (!fp) {
throw std::runtime_error("File not found: " + std::string(path));
}
ByteVec buff;
size_t num_read = 0;
while (feof(fp) == 0)
{
buff.resize(num_read + CHUNK_SIZE);
size_t n = fread(&buff[num_read], 1, CHUNK_SIZE, fp);
num_read += n;
buff.resize(num_read);
}
fclose(fp);
return buff;
}
bool EstEidCard::changePin_internal(
PinType pinType,const PinString &newPin,const PinString &oldPin,bool useUnblockCmd ) {
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
byte cmdChangeCmd[] = {0x00,0x24,0x00};
bool doSecure = false;
//mManager.beginTransaction(mConnection);
if (useUnblockCmd) cmdChangeCmd[1]= 0x2C;
ByteVec cmd(MAKEVECTOR(cmdChangeCmd));
cmd.push_back((byte)pinType);
size_t oldPinLen,newPinLen;
if (newPin.length() < 4 || oldPin.length() < 4 ) {
if (!mConnection->isSecure() )
{
mManager.endTransaction(mConnection);
throw std::runtime_error("bad pin length");
}
doSecure = true;
} else {
oldPinLen = oldPin.length();
newPinLen = newPin.length();
ByteVec catPins;
catPins.resize(oldPinLen + newPinLen);
copy(oldPin.begin(), oldPin.end(), catPins.begin());
copy(newPin.begin(), newPin.end(), catPins.begin() + oldPin.length());
cmd.push_back(LOBYTE(catPins.size()));
cmd.insert(cmd.end(),catPins.begin(),catPins.end());
}
try
{
if (doSecure)
executePinChange(cmd,0,0);
else
execute(cmd);
}
catch(AuthError &ae)
{
mManager.endTransaction(mConnection);
throw AuthError(ae);
}
catch(CardError &e)
{
mManager.endTransaction(mConnection);
if (e.SW1 == 0x63)
throw AuthError(e);
else if (useUnblockCmd && e.SW1==0x6a && e.SW2 == 0x80 ) //unblocking, repeating old pin
throw AuthError(e.SW1,e.SW2,true);
else
throw e;
}
mManager.endTransaction(mConnection);
return true;
}
ByteVec EstEidCard::readEFAndTruncate(unsigned int fileLen)
{
ByteVec ret = readEF(fileLen);
if (ret.size() > 128) { //assume ASN sequence encoding with 2-byte length
size_t realSize = ret[2] * 256 + ret[3] + 4;
ret.resize(realSize);
}
return ret;
}
ByteVec EstEidCard::readEFAndTruncate(unsigned int fileLen)
{
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
ByteVec ret = readEF(fileLen);
if (ret.size() > 128) { //assume ASN sequence encoding with 2-byte length
size_t realSize = ret[2] * 256 + ret[3] + 4;
ret.resize(realSize);
}
return ret;
}
bool EstEidCard::changePin_internal(
PinType pinType,PinString newPin,PinString oldPin,bool useUnblockCmd ) {
byte cmdChangeCmd[] = {0x00,0x24,0x00};
bool doSecure = false;
if (useUnblockCmd) cmdChangeCmd[1]= 0x2C;
ByteVec cmd(MAKEVECTOR(cmdChangeCmd));
cmd.push_back((byte)pinType);
size_t oldPinLen,newPinLen;
if (newPin.length() < 4 || oldPin.length() < 4 ) {
if (!mConnection->isSecure() )
throw std::runtime_error("bad pin length");
// FIXME: This is probably broken on PC/SC readers and
// it is not known to work on CT-API either
oldPinLen = (oldPin[0] - '0') * 10 + oldPin[1] - '0';
newPinLen = (newPin[0] - '0') * 10 + newPin[1]- '0';
oldPin = PinString(oldPinLen,'0');
newPin = PinString(newPinLen,'0');
doSecure = true;
} else {
oldPinLen = oldPin.length();
newPinLen = newPin.length();
}
ByteVec catPins;
catPins.resize(oldPinLen + newPinLen);
copy(oldPin.begin(), oldPin.end(), catPins.begin());
copy(newPin.begin(), newPin.end(), catPins.begin() + oldPin.length());
cmd.push_back(LOBYTE(catPins.size()));
cmd.insert(cmd.end(),catPins.begin(),catPins.end());
try {
if (doSecure)
executePinChange(cmd,oldPinLen,newPinLen);
else
execute(cmd,true);
} catch(AuthError &ae) {
throw AuthError(ae);
} catch(CardError &e) {
if (e.SW1 == 0x63)
throw AuthError(e);
else if (useUnblockCmd && e.SW1==0x6a && e.SW2 == 0x80 ) //unblocking, repeating old pin
throw AuthError(e.SW1,e.SW2,true);
else
throw e;
}
return true;
}
static size_t SDL_RWopsWrite(SDL_RWops *ops, const void *buffer, size_t size, size_t num)
{
ByteVec *v = getRWPrivate(ops);
size_t writeBytes = size * num;
if (writeBytes == 1)
{
char byte = *static_cast<const char*>(buffer);
v->push_back(byte);
return 1;
}
size_t writeInd = v->size();
v->resize(writeInd + writeBytes);
memcpy(&(*v)[writeInd], buffer, writeBytes);
return num;
}
void read(boost::optional<mil_msgs::VelocityMeasurements> &res, boost::optional<mil_msgs::RangeStamped> &height_res)
{
res = boost::none;
height_res = boost::none;
if (!p.is_open()) // Open serial port if closed
{
open();
return;
}
ByteVec ensemble;
ensemble.resize(4);
// Header ID
if (!read_byte(ensemble[0]))
return;
if (ensemble[0] != 0x7F)
return;
ros::Time stamp = ros::Time::now();
// Data Source ID
if (!read_byte(ensemble[1]))
return;
if (ensemble[1] != 0x7F)
return;
// Size low
if (!read_byte(ensemble[2]))
return;
// Size high
if (!read_byte(ensemble[3]))
return;
uint16_t ensemble_size = getu16le(ensemble.data() + 2);
ensemble.resize(ensemble_size);
for (int i = 4; i < ensemble_size; i++)
{
if (!read_byte(ensemble[i]))
return;
}
uint16_t checksum = 0;
BOOST_FOREACH (uint16_t b, ensemble)
checksum += b;
uint16_t received_checksum;
if (!read_short(received_checksum))
return;
if (received_checksum != checksum)
{
ROS_ERROR_THROTTLE(0.5, "DVL: invalid ensemble checksum. received: %i calculated: %i size: %i", received_checksum,
checksum, ensemble_size);
return;
}
if (ensemble.size() < 6)
return;
for (int dt = 0; dt < ensemble[5]; dt++)
{
int offset = getu16le(ensemble.data() + 6 + 2 * dt);
if (ensemble.size() - offset < 2)
continue;
// Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
// Bottom Track, Bottom Track Range
uint16_t section_id = getu16le(ensemble.data() + offset);
std::vector<double> correlations(4, nan(""));
if (section_id == 0x5803) // Bottom Track High Resolution Velocity
{
if (ensemble.size() - offset < 2 + 4 * 4)
continue;
res = boost::make_optional(mil_msgs::VelocityMeasurements());
res->header.stamp = stamp;
std::vector<geometry_msgs::Vector3> dirs;
{
double tilt = 30 * boost::math::constants::pi<double>() / 180;
double x = sin(tilt);
double z = cos(tilt);
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(-x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(+x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
}
// Keep track of which beams didn't return for logging
std::vector<size_t> invalid_beams;
invalid_beams.reserve(4);
for (int i = 0; i < 4; i++)
{
mil_msgs::VelocityMeasurement m;
m.direction = dirs[i];
int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
m.velocity = -vel * .01e-3;
if (vel == -3276801) // -3276801 indicates no data
{
invalid_beams.push_back(i + 1);
m.velocity = nan("");
}
res->velocity_measurements.push_back(m);
}
// Report a list of invalid beams
if (invalid_beams.size() > 0)
{
std::string to_log{ "DVL: didn't return bottom velocity for beam(s): " };
for (auto beam : invalid_beams)
to_log += std::to_string(beam) + " ";
ROS_ERROR_THROTTLE(0.5, "%s", to_log.c_str());
}
}
else if (section_id == 0x0600) // Bottom Track
{
for (int i = 0; i < 4; i++)
correlations[i] = *(ensemble.data() + offset + 32 + i);
}
else if (section_id == 0x5804) // Bottom Track Range
{
if (ensemble.size() - offset < 2 + 4 * 3)
continue;
if (gets32le(ensemble.data() + offset + 10) <= 0)
{
ROS_ERROR_THROTTLE(0.5, "%s", "DVL: didn't return height over bottom");
continue;
}
height_res = boost::make_optional(mil_msgs::RangeStamped());
height_res->header.stamp = stamp;
height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
}
if (res)
{
for (int i = 0; i < 4; i++)
{
res->velocity_measurements[i].correlation = correlations[i];
}
}
}
}
