void
transmitMessage(const std::string& sys, const InlineMessage<IMC::Message>& imsg)
{
if (!hasTransducer())
return;
MicroModemMap::iterator itr = m_ummap.find(sys);
if (itr == m_ummap.end())
return;
const IMC::Message* msg = NULL;
std::string command;
try
{
msg = imsg.get();
}
catch (...)
{
return;
}
if (msg->getId() == DUNE_IMC_PLANCONTROL)
{
const IMC::PlanControl* pc = static_cast<const IMC::PlanControl*>(msg);
Memory::replace(m_pc, new IMC::PlanControl(*pc));
if (pc->op == IMC::PlanControl::PC_START)
{
std::vector<char> pmsg(c_binary_size, 0);
if (pc->plan_id.size() > c_binary_size - 1)
{
err(DTR("unable to send plan %s"), pc->plan_id.c_str());
return;
}
// Make packet.
pmsg[0] = (char)c_code_plan;
std::memcpy(&pmsg[1], &pc->plan_id[0], std::min(c_binary_size - 1, (int)pc->plan_id.size()));
std::string hex = String::toHex(pmsg);
std::string cmd = String::str("$CCTXD,%u,%u,0,%s\r\n",
m_address, itr->second, hex.c_str());
sendCommand(cmd);
std::string cyc = String::str("$CCCYC,0,%u,%u,0,0,1\r\n", m_address, itr->second);
sendCommand(cyc);
}
}
if (command.empty())
return;
sendCommand(command);
}
void
transmitMessage(const std::string& sys, const InlineMessage<IMC::Message>& imsg)
{
if (!hasTransducer())
return;
MicroModemMap::iterator itr = m_ummap.find(sys);
if (itr == m_ummap.end())
return;
const IMC::Message* msg = NULL;
std::string command;
try
{
msg = imsg.get();
}
catch (...)
{
return;
}
if (msg->getId() == DUNE_IMC_PLANCONTROL)
{
const IMC::PlanControl* pc = static_cast<const IMC::PlanControl*>(msg);
Memory::replace(m_pc, new IMC::PlanControl(*pc));
if (pc->op == IMC::PlanControl::PC_START)
{
if (pc->plan_id.size() == 1)
command = String::str("$CCMUC,%u,%u,%04x\r\n", c_plan_addr, itr->second, pc->plan_id[0] & 0xff);
}
}
if (command.empty())
return;
sendCommand(command);
}