text: #include "FlightTask.hpp"
  #include <mathlib/mathlib.h>
  #include <lib/geo/geo.h>
  
  constexpr uint64_t FlightTask::_timeout;
  // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
  const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
  
  const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
  const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
  
  bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
  {
  	_resetSetpoints();
  	_setDefaultConstraints();
  	_time_stamp_activate = hrt_absolute_time();
  	_gear = empty_landing_gear_default_keep;
  	return true;
  }
  
  void FlightTask::reActivate()
  {
  	// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
  	vehicle_local_position_setpoint_s setpoint_preserve_vertical{empty_setpoint};
  	setpoint_preserve_vertical.vz = _velocity_setpoint(2);
  	activate(setpoint_preserve_vertical);
  }
  
  bool FlightTask::updateInitialize()
  {
  	_time_stamp_current = hrt_absolute_time();
  	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
  	_time_stamp_last = _time_stamp_current;
  
  	_sub_vehicle_local_position.update();
  	_sub_home_position.update();
  
  	_evaluateVehicleLocalPosition();
  	_evaluateVehicleLocalPositionSetpoint();
  	_evaluateDistanceToGround();
  	return true;
  }
  
  bool FlightTask::update()
  {
  	_checkEkfResetCounters();
  	return true;
  }
  
  void FlightTask::_checkEkfResetCounters()
  {
  	// Check if a reset event has happened
  	if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
  		_ekfResetHandlerPositionXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_xy});
  		_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
  	}
  
  	if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
  		_ekfResetHandlerVelocityXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_vxy});
  		_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
  	}
  
  	if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
  		_ekfResetHandlerPositionZ(_sub_vehicle_local_position.get().delta_z);
  		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
  	}
  
  	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
  		_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
  		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
  	}
  
  	if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
  		_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
  		_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
  	}
  }
  
  const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
  {
  	/* fill position setpoint message */
  	vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
  	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
  
  	vehicle_local_position_setpoint.x = _position_setpoint(0);
  	vehicle_local_position_setpoint.y = _position_setpoint(1);
  	vehicle_local_position_setpoint.z = _position_setpoint(2);
  
  	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
  	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
  	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
  
  	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
  	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
  
  	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
  	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
  
  	// deprecated, only kept for output logging
  	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
  
  	return vehicle_local_position_setpoint;
  }
  
  void FlightTask::_resetSetpoints()
  {
  	_position_setpoint.setNaN();
  	_velocity_setpoint.setNaN();
  	_acceleration_setpoint.setNaN();
  	_jerk_setpoint.setNaN();
  	_yaw_setpoint = NAN;
  	_yawspeed_setpoint = NAN;
  }
  
  void FlightTask::_evaluateVehicleLocalPosition()
  {
  	_position.setAll(NAN);
  	_velocity.setAll(NAN);
  	_yaw = NAN;
  	_dist_to_bottom = NAN;
  
  	// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
  	if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
  
  		// yaw
  		_yaw = _sub_vehicle_local_position.get().heading;
  		_is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control;
  
  		// 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;
  		}
  
  		if (_sub_vehicle_local_position.get().z_valid) {
  			_position(2) = _sub_vehicle_local_position.get().z;
  		}
  
  		// 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;
  		}
  
  		if (_sub_vehicle_local_position.get().v_z_valid) {
  			_velocity(2) = _sub_vehicle_local_position.get().vz;
  		}
  
  		// distance to bottom
  		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) {
  			if (!map_projection_initialized(&_global_local_proj_ref)
  			    || (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) {
  
  				map_projection_init_timestamped(&_global_local_proj_ref,
  								_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
  								_sub_vehicle_local_position.get().ref_timestamp);
  
  				_global_local_alt0 = _sub_vehicle_local_position.get().ref_alt;
  			}
  		}
  	}
  }
  
  void FlightTask::_evaluateVehicleLocalPositionSetpoint()
  {
  	vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
  
  	// Only use data that is received within a certain timestamp
  	if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)
  	    && (_time_stamp_current - vehicle_local_position_setpoint.timestamp) < _timeout) {
  		// Inform about the input and output of the velocity controller
  		// This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
  		_velocity_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy,
  					      vehicle_local_position_setpoint.vz);
  		_acceleration_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.acceleration);
  
  	} else {
  		_velocity_setpoint_feedback.setAll(NAN);
  		_acceleration_setpoint_feedback.setAll(NAN);
  	}
  }
  
  void FlightTask::_evaluateDistanceToGround()
  {
  	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
  	_dist_to_ground = -_position(2);
  
  	if (PX4_ISFINITE(_dist_to_bottom)) {
  		_dist_to_ground = _dist_to_bottom;
  
  	} else if (_sub_home_position.get().valid_alt) {
  		_dist_to_ground = -(_position(2) - _sub_home_position.get().z);
  	}
  }
  
  void FlightTask::_setDefaultConstraints()
  {
  	_constraints.speed_up = _param_mpc_z_vel_max_up.get();
  	_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
  	_constraints.want_takeoff = false;
  }
  
  bool FlightTask::_checkTakeoff()
  {
  	// position setpoint above the minimum altitude
  	bool position_triggered_takeoff = false;
  
  	if (PX4_ISFINITE(_position_setpoint(2))) {
  		// minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
  		float min_altitude = 0.2f;
  		const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
  
  		if (PX4_ISFINITE(min_distance_to_ground)) {
  			min_altitude = min_distance_to_ground + 0.05f;
  		}
  
  		position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
  	}
  
  	// upwards velocity setpoint
  	bool velocity_triggered_takeoff = false;
  
  	if (PX4_ISFINITE(_velocity_setpoint(2))) {
  		velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
  	}
  
  	return position_triggered_takeoff || velocity_triggered_takeoff;
  }
  