bool FlightTaskFailsafe::update()
{
if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
// stay at current position setpoint
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
// don't move along xy
_position_setpoint(0) = _position_setpoint(1) = NAN;
_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
}
if (PX4_ISFINITE(_position(2))) {
// stay at current altitude setpoint
_velocity_setpoint(2) = 0.0f;
_thrust_setpoint(2) = NAN;
} else if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = MPC_LAND_SPEED.get();
_position_setpoint(2) = NAN;
_thrust_setpoint(2) = NAN;
}
return true;
}
DGFluxBC::DGFluxBC(const InputParameters & parameters) :
IntegratedBC(parameters),
_epsilon(getParam<Real>("epsilon")),
_sigma(getParam<Real>("sigma")),
_vx(getParam<Real>("vx")),
_vy(getParam<Real>("vy")),
_vz(getParam<Real>("vz")),
_Dxx(getParam<Real>("Dxx")),
_Dxy(getParam<Real>("Dxy")),
_Dxz(getParam<Real>("Dxz")),
_Dyx(getParam<Real>("Dyx")),
_Dyy(getParam<Real>("Dyy")),
_Dyz(getParam<Real>("Dyz")),
_Dzx(getParam<Real>("Dzx")),
_Dzy(getParam<Real>("Dzy")),
_Dzz(getParam<Real>("Dzz")),
_u_input(getParam<Real>("u_input"))
{
_velocity(0)=_vx;
_velocity(1)=_vy;
_velocity(2)=_vz;
_Diffusion(0,0) = _Dxx;
_Diffusion(0,1) = _Dxy;
_Diffusion(0,2) = _Dxz;
_Diffusion(1,0) = _Dyx;
_Diffusion(1,1) = _Dyy;
_Diffusion(1,2) = _Dyz;
_Diffusion(2,0) = _Dzx;
_Diffusion(2,1) = _Dzy;
_Diffusion(2,2) = _Dzz;
}
Real GColumnMassAdvection::computeQpJacobian()
{
_velocity(0)=0.0;
_velocity(1)=_vel[_qp];
_velocity(2)=0.0;
return GAdvection::computeQpJacobian();
}
Real GColumnMassAdvection::computeQpResidual()
{
_velocity(0)=0.0;
_velocity(1)=_vel[_qp];
_velocity(2)=0.0;
return GAdvection::computeQpResidual();
}
Real GColumnHeatAdvection::computeQpJacobian()
{
_velocity(0)=0.0;
_velocity(1)=_vel[_qp]*_gas_heat_capacity[_qp]*_gas_density[_qp];
_velocity(2)=0.0;
return GAdvection::computeQpJacobian();
}
DGAdvection::DGAdvection(const InputParameters & parameters) :
DGKernel(parameters),
_vx(getParam<Real>("vx")),
_vy(getParam<Real>("vy")),
_vz(getParam<Real>("vz"))
{
_velocity(0)=_vx;
_velocity(1)=_vy;
_velocity(2)=_vz;
}
/// @brief Create a twist using a twistcoordinates vector.
/// @param V the twist coordinates.
/// @note V(0)-V(2) correspond to the velocity, and V(3)-V(5) to the rotation.
Twist(const TwistCoordinates<NumType>& V)
{
_velocity(0) = V(0);
_velocity(1) = V(1);
_velocity(2) = V(2);
_skew = Skew<NumType>(Translation<NumType>(V(3), V(4), V(5)));
_zeroVal = (NumType)0;
}
Real
DGColumnHeatAdvection::computeQpJacobian(Moose::DGJacobianType type)
{
_velocity(0)=0.0;
_velocity(1)=_vel[_qp]*_gas_heat_capacity[_qp]*_gas_density[_qp];
_velocity(2)=0.0;
return DGAdvection::computeQpJacobian(type);
}
DGFunctionConvectionDirichletBC::DGFunctionConvectionDirichletBC(const InputParameters & parameters) :
IntegratedBC(parameters),
_func(getFunction("function")),
_x(getParam<Real>("x")),
_y(getParam<Real>("y")),
_z(getParam<Real>("z"))
{
_velocity(0) = _x;
_velocity(1) = _y;
_velocity(2) = _z;
}
MatConvection::MatConvection(const std::string & name, InputParameters parameters) :
Kernel(name, parameters),
_conv_prop(getMaterialProperty<Real>("mat_prop")),
_x(getParam<Real>("x")),
_y(getParam<Real>("y")),
_z(getParam<Real>("z"))
{
_velocity(0) = _x;
_velocity(1) = _y;
_velocity(2) = _z;
}
/// @brief Create a twist using the 6 twist coordinates.
/// @param t0-t6: the twist coordinates.
Twist(const NumType& t0, const NumType& t1, const NumType& t2,
const NumType& t3, const NumType& t4, const NumType& t5)
{
_velocity(0) = t0;
_velocity(1) = t1;
_velocity(2) = t2;
_skew = Skew<NumType>(Translation<NumType>(t3, t4, t5));
_zeroVal = (NumType)0;
}
Real
DGHeatFluxBC::computeQpJacobian()
{
_velocity(0)=0.0;
_velocity(1)=_vel[_qp]*_gas_heat_capacity[_qp]*_gas_density[_qp];
_velocity(2)=0.0;
_Diffusion(0,0) = _conductivity[_qp];
_Diffusion(0,1) = std::pow(std::pow(_conductivity[_qp],2.0) + std::pow(_conductivity[_qp],2.0),0.5);
_Diffusion(0,2) = 0.0;
_Diffusion(1,0) = std::pow(std::pow(_conductivity[_qp],2.0) + std::pow(_conductivity[_qp],2.0),0.5);
_Diffusion(1,1) = _conductivity[_qp];
_Diffusion(1,2) = 0.0;
_Diffusion(2,0) = 0.0;
_Diffusion(2,1) = 0.0;
_Diffusion(2,2) = 0.0;
_u_input = _input_temperature;
return DGFluxBC::computeQpJacobian();
}
Real
DGMassFluxLimitedBC::computeQpJacobian()
{
_velocity(0)=0.0;
_velocity(1)=_vel[_qp];
_velocity(2)=0.0;
_Diffusion(0,0) = _molecular_diffusion[_qp][_index];
_Diffusion(0,1) = std::pow(std::pow(_molecular_diffusion[_qp][_index],2.0) + std::pow(_dispersion[_qp][_index],2.0),0.5);
_Diffusion(0,2) = 0.0;
_Diffusion(1,0) = std::pow(std::pow(_molecular_diffusion[_qp][_index],2.0) + std::pow(_dispersion[_qp][_index],2.0),0.5);
_Diffusion(1,1) = _dispersion[_qp][_index];
_Diffusion(1,2) = 0.0;
_Diffusion(2,0) = 0.0;
_Diffusion(2,1) = 0.0;
_Diffusion(2,2) = 0.0;
_u_input = (_input_pressure * _input_molefraction) / (8.3144621 * _input_temperature);
return DGFluxLimitedBC::computeQpJacobian();
}
/// @brief Return the requested element (read).
/// @param i the index of the row.
/// @param j the index of the column.
/// @throw scews::ScrewException for out of bounds indices.
const NumType& operator () (const unsigned int& i, const unsigned int& j) const
{
assert(i >= 0 && i < 4 && j >= 0 && j < 4);
if (i == 3)
{
return _zeroVal;
}
else if (j < 3)
{
return _skew(i, j);
}
else if (j == 3)
{
return _velocity(i);
}
else
{
screws::ScrewException s("Index out of bounds.", __FILE__, __FUNCTION__, __LINE__);
throw s;
}
}
bool FlightTask::_evaluateVehicleLocalPosition()
{
if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
// position
if (_sub_vehicle_local_position->get().xy_valid) {
_position(0) = _sub_vehicle_local_position->get().x;
_position(1) = _sub_vehicle_local_position->get().y;
} else {
_position(0) = _position(1) = NAN;
}
if (_sub_vehicle_local_position->get().z_valid) {
_position(2) = _sub_vehicle_local_position->get().z;
} else {
_position(2) = NAN;
}
// velocity
if (_sub_vehicle_local_position->get().v_xy_valid) {
_velocity(0) = _sub_vehicle_local_position->get().vx;
_velocity(1) = _sub_vehicle_local_position->get().vy;
} else {
_velocity(0) = _velocity(1) = NAN;
}
if (_sub_vehicle_local_position->get().v_z_valid) {
_velocity(2) = _sub_vehicle_local_position->get().vz;
} else {
_velocity(2) = NAN;
}
// yaw
_yaw = _sub_vehicle_local_position->get().yaw;
// distance to bottom
_dist_to_bottom = NAN;
if (_sub_vehicle_local_position->get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {
_dist_to_bottom = _sub_vehicle_local_position->get().dist_bottom;
}
// global frame reference coordinates to enable conversions
if (_sub_vehicle_local_position->get().xy_global && _sub_vehicle_local_position->get().z_global) {
globallocalconverter_init(_sub_vehicle_local_position->get().ref_lat, _sub_vehicle_local_position->get().ref_lon,
_sub_vehicle_local_position->get().ref_alt, _sub_vehicle_local_position->get().ref_timestamp);
}
// We don't check here if states are valid or not.
// Validity checks are done in the sub-classes.
return true;
} else {
_resetSetpoints();
return false;
}
}
