/**
* @brief Write always performes to shadow bank
*/
bool WpDB::write(const mavlink_mission_item_t *wpp, uint16_t seq) {
size_t bytecnt = 0;
const size_t shadow_bank = next_bank(active_bank);
chDbgCheck(nullptr != this->dbfile);
chDbgCheck(nullptr != wpp);
memcpy(buf, wpp, sizeof(*wpp));
seal_with_crc(buf);
/* actual write */
dbfile->setPosition(calc_offset(seq, shadow_bank));
bytecnt = dbfile->write(buf, WAYPOINT_FOOTPRINT);
if (WAYPOINT_FOOTPRINT != bytecnt)
return OSAL_FAILED;
/* read back and verify checksum */
dbfile->setPosition(calc_offset(seq, shadow_bank));
bytecnt = dbfile->read(buf, WAYPOINT_FOOTPRINT);
if (WAYPOINT_FOOTPRINT != bytecnt)
return OSAL_FAILED;
if (crc_valid(buf)) {
shadow_count++;
return OSAL_SUCCESS;
}
else {
return OSAL_FAILED;
}
}
uint16_t WpDB::start(void) {
size_t readcnt;
dbfile = NvramTryOpen(WPDB_FILE_NAME, BOOTSTRAP_WPDB_FILE_SIZE);
osalDbgCheck(nullptr != dbfile);
/* precalculate capacity for single bank */
this->capacity = (dbfile->getSize() - HEADER_SIZE) / (WAYPOINT_FOOTPRINT * 2);
dbfile->setPosition(0);
dbfile->get(&count);
this->shadow_count = 0;
/* deduce what bank contains valid data and adjust 'count' when needed */
if (0 == (count & (1U << 15))){
active_bank = 0;
}
else {
active_bank = 1;
count &= ~(1U << 15);
}
/* validate data in current bank */
for (size_t seq=0; seq<count; seq++) {
dbfile->setPosition(calc_offset(seq, active_bank));
readcnt = dbfile->read(buf, WAYPOINT_FOOTPRINT);
if (WAYPOINT_FOOTPRINT != readcnt)
goto FAILED;
if (! crc_valid(buf)) {
goto FAILED;
}
}
return count;
FAILED:
count = 0;
return 0;
}
bool WpDB::read(mavlink_mission_item_t *wpp, uint16_t seq) {
size_t result;
osalDbgCheck(nullptr != this->dbfile);
osalDbgCheck(nullptr != wpp);
osalDbgCheck(active_bank <= 1);
if (seq >= count)
return OSAL_FAILED;
else {
dbfile->setPosition(calc_offset(seq, active_bank));
result = dbfile->read(buf, WAYPOINT_FOOTPRINT);
if (WAYPOINT_FOOTPRINT != result)
return OSAL_FAILED;
if (! crc_valid(buf))
return OSAL_FAILED;
memcpy(wpp, buf, sizeof(*wpp));
return OSAL_SUCCESS;
}
}
bool SerialInterface::getPackets (Telemetry * telemetry)
{
flush ();
ROS_DEBUG ("SerialInterface::getPackets");
char cmd[16];
char spacket[1024];
unsigned char packet_type;
unsigned short packet_crc;
unsigned short packet_size;
unsigned int i;
bool result = false;
ros::Time packetTime;
ROS_DEBUG (" Requesting %04x %zd packets", (short) telemetry->requestPackets_.to_ulong (),
telemetry->requestPackets_.count ());
sprintf (cmd, ">*>p%c", (short) telemetry->requestPackets_.to_ulong ());
output (cmd, 6);
for (i = 0; i < telemetry->requestPackets_.count (); i++)
{
packetTime = ros::Time::now(); // Presumes that the AutoPilot is grabbing the data for each packet
// immediately prior to each packet being sent, as opposed to gathering
// all the data at once and then sending it all. Either way is a guess
// unless we get some info from AscTec one way or the other..
bool read_result = getPacket (spacket, packet_type, packet_crc, packet_size);
if (read_result)
{
ROS_DEBUG (" Read successful: type = %d, crc = %d", packet_type, packet_crc);
if (packet_type == Telemetry::PD_LLSTATUS)
{
ROS_DEBUG (" Packet type is LL_STATUS");
memcpy (&telemetry->LL_STATUS_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::LL_STATUS] = packetTime;
if (crc_valid (packet_crc, &telemetry->LL_STATUS_, sizeof (packet_size)))
{
result = true;
}
//telemetry->dumpLL_STATUS();
}
else if (packet_type == Telemetry::PD_IMURAWDATA)
{
ROS_DEBUG (" Packet type is IMU_RAWDATA");
memcpy (&telemetry->IMU_RAWDATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::IMU_RAWDATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->IMU_RAWDATA_, packet_size))
{
result = true;
}
//telemetry->dumpIMU_RAWDATA();
}
else if (packet_type == Telemetry::PD_IMUCALCDATA)
{
ROS_DEBUG (" Packet type is IMU_CALCDATA");
memcpy (&telemetry->IMU_CALCDATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::IMU_CALCDATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->IMU_CALCDATA_, packet_size))
{
result = true;
}
//telemetry->dumpIMU_CALCDATA();
}
else if (packet_type == Telemetry::PD_RCDATA)
{
ROS_DEBUG (" Packet type is RC_DATA");
memcpy (&telemetry->RC_DATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::RC_DATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->RC_DATA_, packet_size))
{
result = true;
}
//telemetry->dumpRC_DATA();
}
else if (packet_type == Telemetry::PD_CTRLOUT)
{
ROS_DEBUG (" Packet type is CONTROLLER_OUTPUT");
memcpy (&telemetry->CONTROLLER_OUTPUT_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::CONTROLLER_OUTPUT] = packetTime;
if (crc_valid (packet_crc, &telemetry->CONTROLLER_OUTPUT_, packet_size))
{
result = true;
}
//telemetry->dumpCONTROLLER_OUTPUT();
}
else if (packet_type == Telemetry::PD_GPSDATA)
{
ROS_DEBUG (" Packet type is GPS_DATA");
memcpy (&telemetry->GPS_DATA_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::GPS_DATA] = packetTime;
if (crc_valid (packet_crc, &telemetry->GPS_DATA_, packet_size))
{
result = true;
}
//telemetry->dumpGPS_DATA();
}
else if (packet_type == Telemetry::PD_GPSDATAADVANCED)
{
ROS_DEBUG (" Packet type is GPS_DATA_ADVANCED");
memcpy (&telemetry->GPS_DATA_ADVANCED_, spacket, packet_size);
telemetry->timestamps_[RequestTypes::GPS_DATA_ADVANCED] = packetTime;
if (crc_valid (packet_crc, &telemetry->GPS_DATA_ADVANCED_, packet_size))
{
result = true;
}
//telemetry->dumpGPS_DATA_ADVANCED();
}
else
{
ROS_ERROR (" Packet type (%#2x) is UNKNOWN", packet_type);
}
}
else
{
// failed read
ROS_ERROR (" Read failed");
break;
}
}
ioctl(dev_,FIONREAD,&i);
if (i != 0)
{
ROS_ERROR (" Unexpected Data: Flushing receive buffer");
flush();
}
return result;
}
