//============================================================================== 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); } } }