//==============================================================================
void IniParser::onValue(Json §ion, const std::string &line) noexcept(false)
{
static constexpr std::uint32_t size = 2;
std::vector<std::string> nameValue = String::Split(line, '=', size);
if (nameValue.size() == size)
{
std::string name(nameValue[0]), value(nameValue[1]);
String::Trim(name);
String::Trim(value);
if (trimValue(name, '\'') || trimValue(name, '\"'))
{
throw ParserException(m_line, "Value names must not be quoted");
}
trimValue(value, '\'');
trimValue(value, '\"');
section[name] = value;
}
else
{
throw ParserException(
m_line, "Require name/value pairs of the form name=value");
}
}
//! Compute a plan's progress from maneuver and ManeuverControlState
//! @param[in] man pointer to maneuver message
//! @param[in] mcs pointer to ManeuverControlState message
//! @param[in] durations pointer to vector of maneuver duration(s)
//! @param[in] total_duration full duration of the plan
//! @return progress of the whole plan
static float
compute(const IMC::Message* man, const IMC::ManeuverControlState* mcs,
const std::vector<float>& durations, float total_duration)
{
if (man == NULL)
return -1.0;
if (!durations.size() || mcs->eta == c_max_eta)
return -1.0;
float time_left = -1.0;
switch (man->getId())
{
#ifdef DUNE_IMC_GOTO
case DUNE_IMC_GOTO:
#endif
#ifdef DUNE_IMC_STATIONKEEPING
case DUNE_IMC_STATIONKEEPING:
#endif
#ifdef DUNE_IMC_LOITER
case DUNE_IMC_LOITER:
#endif
#ifdef DUNE_IMC_ELEVATOR
case DUNE_IMC_ELEVATOR:
#endif
#ifdef DUNE_IMC_YOYO
case DUNE_IMC_YOYO:
#endif
#ifdef DUNE_IMC_POPUP
case DUNE_IMC_POPUP:
time_left = total_duration - durations[0] + (float)mcs->eta;
break;
#endif
#ifdef DUNE_IMC_FOLLOWPATH
case DUNE_IMC_FOLLOWPATH:
time_left = compute(dynamic_cast<const IMC::FollowPath*>(man), mcs,
durations, total_duration);
break;
#endif
#ifdef DUNE_IMC_ROWS
case DUNE_IMC_ROWS:
time_left = compute(dynamic_cast<const IMC::Rows*>(man), mcs,
durations, total_duration);
break;
#endif
default:
return -1.0;
}
if (time_left < 0.0)
return -1.0;
time_left = trimValue(time_left, 0.0, total_duration);
return 100.0 * trimValue((1.0 - time_left / total_duration), 0.0, 1.0);
};
//==============================================================================
std::string IniParser::onSection(const std::string &line) noexcept(false)
{
std::string section = line;
String::Trim(section);
if (trimValue(section, '\'') || trimValue(section, '\"'))
{
throw ParserException(m_line, "Section names must not be quoted");
}
return section;
}
//==============================================================================
Json IniParser::ParseInternal(std::istream &stream) noexcept(false)
{
std::string line, section;
Json values;
while (stream.good() && std::getline(stream, line))
{
String::Trim(line);
++m_line;
if (line.empty() || String::StartsWith(line, ';'))
{
// Ignore comments and blank lines
}
else if (trimValue(line, '[', ']'))
{
section = onSection(line);
}
else if (!section.empty())
{
onValue(values[section], line);
}
else
{
throw ParserException(
m_line, "A section must be defined before name=value pairs");
}
}
return values;
}
void
consume(const IMC::SimulatedState* msg)
{
(void)msg;
if (!m_args.limit_rate)
return;
float max_rot = (Clock::get() - m_last_time) * m_args.max_rate;
for (unsigned i = 0; i < c_servo_count; ++i)
{
if (m_servo_in_fault != (int)i)
{
float diff = m_commands[i].value - m_positions[i].value;
diff = trimValue(diff, -max_rot, max_rot);
m_positions[i].value += diff;
}
dispatch(m_positions[i]);
}
m_last_time = Clock::get();
}
void
consume(const IMC::SetServoPosition* msg)
{
if (!m_args.limit_rate)
{
m_positions[msg->id].value = trimValue(msg->value,
-m_args.max_angle,
m_args.max_angle);
dispatch(m_positions[msg->id]);
}
else
{
m_last_time = Clock::get();
m_commands[msg->id].value = trimValue(msg->value,
-m_args.max_angle,
m_args.max_angle);
}
if (m_args.generate_faults && !m_faulted)
{
if (m_fault_timer < 0.0)
{
m_fault_timer = Clock::get();
}
else if (Clock::get() - m_fault_timer > m_args.fault_trigger)
{
m_servo_in_fault = Math::roundToInteger(m_prng->uniform(0, c_servo_count - 1));
m_faulted = true;
war(DTR("fault triggered in servo #%d"), m_servo_in_fault);
}
}
else if (m_args.generate_faults && (m_servo_in_fault >= 0))
{
if (Clock::get() - m_fault_timer > c_fault_duration)
{
war(DTR("servo #%d is no longer in fault"), m_servo_in_fault);
m_servo_in_fault = -1;
}
}
}
//! Dispatch to bus SetThrusterActuation message
//! @param[in] value set thrust actuation value
//! @param[in] timestep amount of time since last control step
void
dispatchThrust(float value, double timestep)
{
if (!m_braking)
{
if ((value > m_last_act.value) && (m_args.ramp_act > 0.0))
{
value = m_last_act.value + trimValue((value - m_last_act.value) / timestep,
0.0, m_args.ramp_act * timestep);
}
m_act.value = trimValue(value, -1, 1);
dispatch(m_act);
}
else
{
m_act.value = 0.0;
dispatch(m_act);
}
m_last_act.value = m_act.value;
}
void
task(void)
{
// Set servo positions.
uint8_t data[c_servo_count + 1];
data[0] = 0;
for (unsigned i = 0; i < c_servo_count; ++i)
{
unsigned nr = m_args.servo_renumbering[i];
float value = m_set_position[nr];
// compute elapsed time to trim according to max rotation rate
double elapsedtime = Clock::get() - m_last_timestamp[nr];
if (elapsedtime > 0 && m_args.limit_servo_rate)
{
elapsedtime = trimValue(elapsedtime, 0, (m_args.servo_max - m_args.servo_min) / m_args.servo_rate_max);
if (value - m_last_ref[nr] >= 0)
value = std::min((double)value, m_last_ref[nr] + m_args.servo_rate_max * elapsedtime);
else
value = std::max((double)value, m_last_ref[nr] - m_args.servo_rate_max * elapsedtime);
}
// trim according to max and min rotation
value = trimValue(value, m_args.servo_min, m_args.servo_max);
// update variables used as previous
m_last_ref[nr] = value;
m_last_timestamp[nr] = Clock::get();
int ticks = (int)(256 + m_args.servo_orient[nr] * (value + m_args.servo_middle[nr]) * (400.0 / DUNE::Math::c_pi));
m_servo_ref[nr] = trimValue(ticks, 0, 511);
data[0] |= (m_servo_ref[nr] & 0x100) >> (8 - i);
data[i + 1] = m_servo_ref[nr] & 0xFF;
}
m_proto.sendCommand(CMD_SERVO_SET, data, c_servo_count + 1);
if (!waitForCommand(CMD_SERVO_SET))
{
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
throw RestartNeeded(DTR(Status::getString(Status::CODE_COM_ERROR)), 5);
}
else
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
if (!m_last_adc.overflow())
return;
m_last_adc.reset();
// Request state.
m_proto.sendCommand(CMD_STATE);
if (!waitForCommand(CMD_STATE))
{
setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
}
else
{
setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
}
}
//==============================================================================
bool IniParser::trimValue(std::string &str, char ch) const noexcept(false)
{
return trimValue(str, ch, ch);
}
float
Plan::progress(const IMC::ManeuverControlState* mcs)
{
if (!m_compute_progress)
return -1.0;
// Compute only if linear and durations exists
if (!isLinear() || !m_profiles->size())
return -1.0;
// If calibration has not started yet, but will later
if (m_calib->notStarted())
return -1.0;
float total_duration = getTotalDuration();
float exec_duration = getExecutionDuration();
// Check if its calibrating
if (m_calib->inProgress())
{
float time_left = m_calib->getRemaining() + exec_duration;
m_progress = 100.0 * trimValue(1.0 - time_left / total_duration, 0.0, 1.0);
return m_progress;
}
// If it's not executing, do not compute
if (mcs->state != IMC::ManeuverControlState::MCS_EXECUTING ||
mcs->eta == 0)
return m_progress;
TimeProfile::const_iterator itr;
itr = m_profiles->find(getCurrentId());
// If not found
if (itr == m_profiles->end())
{
// If beyond the last maneuver with valid duration
if (m_beyond_dur)
{
m_progress = 100.0;
return m_progress;
}
else
{
return -1.0;
}
}
// If durations vector for this maneuver is empty
if (!itr->second.durations.size())
return m_progress;
IMC::Message* man = m_graph.find(getCurrentId())->second.pman->data.get();
// Get execution progress
float exec_prog = Progress::compute(man, mcs, itr->second.durations, exec_duration);
float prog = 100.0 - getExecutionPercentage() * (1.0 - exec_prog / 100.0);
// If negative, then unable to compute
// But keep last value of progress if it is not invalid
if (prog < 0.0)
{
if (m_progress < 0.0)
return -1.0;
else
return m_progress;
}
// Never output shorter than previous
m_progress = prog > m_progress ? prog : m_progress;
return m_progress;
}
bool
Plan::parse(std::string& desc, const std::set<uint16_t>* supported_maneuvers,
bool plan_startup, const std::map<std::string, IMC::EntityInfo>& cinfo,
Tasks::Task* task, const IMC::EstimatedState* state)
{
bool start_maneuver_ok = false;
clear();
if (!m_spec->maneuvers.size())
{
desc = m_spec->plan_id + DTR(": no maneuvers");
return false;
}
IMC::MessageList<IMC::PlanManeuver>::const_iterator mitr;
mitr = m_spec->maneuvers.begin();
// parse maneuvers and transitions
do
{
if (*mitr == NULL)
{
++mitr;
continue;
}
if ((*mitr)->data.isNull())
{
desc = (*mitr)->maneuver_id + DTR(": actual maneuver not specified");
return false;
}
const IMC::Message* m = (*mitr)->data.get();
if (supported_maneuvers->find(m->getId()) == supported_maneuvers->end())
{
desc = (*mitr)->maneuver_id + DTR(": maneuver is not supported");
return false;
}
if ((*mitr)->maneuver_id == m_spec->start_man_id)
start_maneuver_ok = true;
Node node;
bool matched = false;
node.pman = (*mitr);
IMC::MessageList<IMC::PlanTransition>::const_iterator tritr;
tritr = m_spec->transitions.begin();
for (; tritr != m_spec->transitions.end(); ++tritr)
{
if (*tritr == NULL)
continue;
if ((*tritr)->dest_man == (*mitr)->maneuver_id)
matched = true;
if ((*tritr)->source_man == (*mitr)->maneuver_id)
node.trans.push_back((*tritr));
}
// if a match was not found and this is not the start maneuver
if (!matched && ((*mitr)->maneuver_id != m_spec->start_man_id))
{
std::string str = DTR(": maneuver has no incoming transition"
" and it's not the initial maneuver");
desc = (*mitr)->maneuver_id + str;
return false;
}
m_graph[(*mitr)->maneuver_id] = node;
++mitr;
}
while (mitr != m_spec->maneuvers.end());
if (!start_maneuver_ok)
{
desc = m_spec->start_man_id + DTR(": invalid start maneuver");
return false;
}
if (m_compute_progress && plan_startup)
{
sequenceNodes();
if (m_sequential && state != NULL)
{
computeDurations(state);
Memory::clear(m_sched);
m_sched = new ActionSchedule(task, m_spec, m_seq_nodes,
*m_durations, m_last_dur, cinfo);
// Estimate necessary calibration time
float diff = m_sched->getEarliestSchedule() - getExecutionDuration();
m_est_cal_time = (uint16_t)trimValue(diff, 0.0, diff);
m_est_cal_time = (uint16_t)std::max(m_min_cal_time, m_est_cal_time);
}
else if (!m_sequential)
{
Memory::clear(m_sched);
m_sched = new ActionSchedule(task, m_spec, m_seq_nodes, cinfo);
m_est_cal_time = m_min_cal_time;
}
}
m_last_id = m_spec->start_man_id;
return true;
}
void HDDModePageReader::read() {
parameterList = std::vector<struct KeyValuePair>();
std::string cmd = std::string("sginfo -A ") + device;
FILE* pipe = popen(cmd.c_str(), "r");
if (!pipe) return;
char buffer[128];
std::string result = "";
while(!feof(pipe)) {
if(fgets(buffer, 128, pipe) != NULL)
result += buffer;
}
pclose(pipe);
// split result by line and save each line in resultLines
std::vector<std::string> resultLines = std::vector<std::string>();
size_t curStart = 0;
size_t pos = result.find_first_of('\n', curStart);
while(pos < result.length()) {
resultLines.push_back(result.substr(curStart, pos-curStart));
curStart = pos+1;
pos = result.find_first_of('\n', curStart);
}
// extract information
for(unsigned long long int i = 0; i < resultLines.size(); i++) {
if(resultLines[i].empty()) {
continue;
} else if(resultLines[i+1].find("-----", 0) != std::string::npos || resultLines[i].find("-----", 0) != std::string::npos) {
continue;
} else {
struct KeyValuePair newPair;
if(resultLines[i].find("Vendor", 0) != std::string::npos || resultLines[i].find("Product", 0) != std::string::npos || resultLines[i].find("Revision level", 0) != std::string::npos) {
// divide after 28 chars
newPair.key = resultLines[i].substr(0, 27);
newPair.value = resultLines[i].substr(27, 36);
} else if(resultLines[i].find("Serial Number", 0) != std::string::npos) {
// divide after 16 chars
newPair.key = resultLines[i].substr(0, 15);
newPair.value = resultLines[i].substr(15, 36);
} else {
// divide after 32 chars
newPair.key = resultLines[i].substr(0, 35);
newPair.value = resultLines[i].substr(35, 36);
}
trimValue(&newPair.key);
trimValue(&newPair.value);
// save most relevant data seperately
if(newPair.key.find("Vendor") != std::string::npos) {
vendor = newPair.value;
} else if(newPair.key.find("Product") != std::string::npos) {
deviceName = newPair.value;
} else if(newPair.key.find("Revision level") != std::string::npos) {
revision = newPair.value;
}
parameterList.push_back(newPair);
}
}
}
