void BasicDeviceDriver::consume(const IMC::EstimatedState* msg) { if (msg->getSource() != getSystemId()) return; if (!isActive()) return; onEstimatedState(*msg); }
void BasicUAVAutopilot::consume(const IMC::EstimatedState* msg) { if (!isActive()) return; m_estate = *msg; // Compute time delta. float timestep = m_last_estate.getDelta(); // Check if we have a valid time delta. if (timestep < 0.0) return; onEstimatedState(timestep, msg); }
void BasicAutopilot::consume(const IMC::EstimatedState* msg) { if (!isActive()) return; if (msg->getSource() != getSystemId()) return; m_estate = *msg; // check if vertical control mode is valid if (m_vertical_mode >= VERTICAL_MODE_SIZE) { signalBadVertical(DTR("invalid vertical control mode %d")); return; } else if (m_vertical_mode == VERTICAL_MODE_NONE) { float delta = m_vmode_delta.getDelta(); if (delta > 0.0) m_vmode_wait += delta; if (m_vmode_wait > c_mode_timeout) { m_vmode_wait = 0.0; signalBadVertical(DTR("timed out while waiting for vertical control mode")); } return; } else if (m_vertical_mode == VERTICAL_MODE_ALTITUDE) { // Required depth for bottom following = current depth + required altitude correction // in case the follow depth is lower than 0.0 it will be capped since the btrack // is not possible // check if we have altitude measurements if (msg->alt < 0.0) { setEntityState(IMC::EntityState::ESTA_ERROR, DTR(c_no_alt)); err("%s", DTR(c_no_alt)); return; } m_bottom_follow_depth = m_estate.depth + (msg->alt - m_vertical_ref); if (m_bottom_follow_depth < 0.0) { if (m_btrack_wrn.overflow()) { std::string desc = String::str(DTR("water column not deep enough for altitude control ( %0.2f < %0.2f )"), msg->alt + m_estate.depth, m_vertical_ref); setEntityState(IMC::EntityState::ESTA_NORMAL, desc); war("%s", desc.c_str()); m_btrack_wrn.reset(); } } else { m_btrack_wrn.reset(); } // Will not let the bottom follow depth be lower than zero // to avoid causing excessive controller integration m_bottom_follow_depth = std::max(m_bottom_follow_depth, (float)0.0); } else if (m_vertical_mode == VERTICAL_MODE_PITCH) { if (m_estate.depth >= m_max_depth - c_depth_margin) { const std::string desc = DTR("getting too close to maximum admissible depth"); setEntityState(IMC::EntityState::ESTA_ERROR, desc); err("%s", desc.c_str()); requestDeactivation(); return; } } // check if yaw control mode is valid if (m_yaw_mode >= YAW_MODE_SIZE) { signalBadYaw(DTR("invalid yaw control mode %d")); return; } else if (m_yaw_mode == YAW_MODE_NONE) { float delta = m_ymode_delta.getDelta(); if (delta > 0.0) m_ymode_wait += delta; if (m_ymode_wait > c_mode_timeout) { m_ymode_wait = 0.0; signalBadYaw(DTR("timed out waiting for yaw control mode")); } return; } // Compute time delta. float timestep = m_last_estate.getDelta(); // Check if we have a valid time delta. if (timestep < 0.0) return; onEstimatedState(timestep, msg); }