void
FixedwingAttitudeControl::task_main()
{
	/*
	 * do subscriptions
	 */
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vcontrol_mode_sub, 200);
	/* do not limit the attitude updates in order to minimize latency.
	 * actuator outputs are still limited by the individual drivers
	 * properly to not saturate IO or physical limitations */

	parameters_update();

	/* get an initial update for all sensor and status data */
	vehicle_airspeed_poll();
	vehicle_setpoint_poll();
	vehicle_accel_poll();
	vehicle_control_mode_poll();
	vehicle_manual_poll();
	vehicle_status_poll();

	/* wakeup source(s) */
	struct pollfd fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _att_sub;
	fds[1].events = POLLIN;

	_task_running = true;

	while (!_task_should_exit) {

		static int loop_counter = 0;

		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run controller if attitude changed */
		if (fds[1].revents & POLLIN) {


			static uint64_t last_run = 0;
			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too large deltaT's */
			if (deltaT > 1.0f)
				deltaT = 0.01f;

			/* load local copies */
			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);

			if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
				 *
				 * Since the VTOL airframe is initialized as a multicopter we need to
				 * modify the estimated attitude for the fixed wing operation.
				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
				 * the pitch axis compared to the neutral position of the vehicle in multicopter mode
				 * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
				 * Additionally, in order to get the correct sign of the pitch, we need to multiply
				 * the new x axis of the rotation matrix with -1
				 *
				 * original:			modified:
				 *
				 * Rxx  Ryx  Rzx		-Rzx  Ryx  Rxx
				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
				 * */
				math::Matrix<3, 3> R;				//original rotation matrix
				math::Matrix<3, 3> R_adapted;		//modified rotation matrix
				R.set(_att.R);
				R_adapted.set(_att.R);

				/* move z to x */
				R_adapted(0, 0) = R(0, 2);
				R_adapted(1, 0) = R(1, 2);
				R_adapted(2, 0) = R(2, 2);

				/* move x to z */
				R_adapted(0, 2) = R(0, 0);
				R_adapted(1, 2) = R(1, 0);
				R_adapted(2, 2) = R(2, 0);

				/* change direction of pitch (convert to right handed system) */
				R_adapted(0, 0) = -R_adapted(0, 0);
				R_adapted(1, 0) = -R_adapted(1, 0);
				R_adapted(2, 0) = -R_adapted(2, 0);
				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
				euler_angles = R_adapted.to_euler();

				/* fill in new attitude data */
				_att.roll    = euler_angles(0);
				_att.pitch   = euler_angles(1);
				_att.yaw     = euler_angles(2);
				PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
				PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
				PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
				PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
				PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
				PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
				PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);

				/* lastly, roll- and yawspeed have to be swaped */
				float helper = _att.rollspeed;
				_att.rollspeed = -_att.yawspeed;
				_att.yawspeed = helper;
			}

			vehicle_airspeed_poll();

			vehicle_setpoint_poll();

			vehicle_accel_poll();

			vehicle_control_mode_poll();

			vehicle_manual_poll();

			global_pos_poll();

			vehicle_status_poll();

			/* lock integrator until control is started */
			bool lock_integrator;

			if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
				lock_integrator = false;

			} else {
				lock_integrator = true;
			}

			/* Simple handling of failsafe: deploy parachute if failsafe is on */
			if (_vcontrol_mode.flag_control_termination_enabled) {
				_actuators_airframe.control[7] = 1.0f;
				//warnx("_actuators_airframe.control[1] = 1.0f;");
			} else {
				_actuators_airframe.control[7] = 0.0f;
				//warnx("_actuators_airframe.control[1] = -1.0f;");
			}

			/* default flaps to center */
			float flaps_control = 0.0f;

			/* map flaps by default to manual if valid */
			if (isfinite(_manual.flaps)) {
				flaps_control = _manual.flaps;
			}

			/* decide if in stabilized or full manual control */

			if (_vcontrol_mode.flag_control_attitude_enabled) {

				/* scale around tuning airspeed */

				float airspeed;

				/* if airspeed is not updating, we assume the normal average speed */
				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
					airspeed = _parameters.airspeed_trim;
					if (nonfinite) {
						perf_count(_nonfinite_input_perf);
					}
				} else {
					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
				}

				/*
				 * For scaling our actuators using anything less than the min (close to stall)
				 * speed doesn't make any sense - its the strongest reasonable deflection we
				 * want to do in flight and its the baseline a human pilot would choose.
				 *
				 * Forcing the scaling to this value allows reasonable handheld tests.
				 */

				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);

				float roll_sp = _parameters.rollsp_offset_rad;
				float pitch_sp = _parameters.pitchsp_offset_rad;
				float yaw_manual = 0.0f;
				float throttle_sp = 0.0f;

				/* Read attitude setpoint from uorb if
				 * - velocity control or position control is enabled (pos controller is running)
				 * - manual control is disabled (another app may send the setpoint, but it should
				 *   for sure not be set from the remote control values)
				 */
				if (_vcontrol_mode.flag_control_auto_enabled ||
						!_vcontrol_mode.flag_control_manual_enabled) {
					/* read in attitude setpoint from attitude setpoint uorb topic */
					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}
					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}
					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
					}
				} else if (_vcontrol_mode.flag_control_velocity_enabled) {

					/* the pilot does not want to change direction,
					 * take straight attitude setpoint from position controller
					 */
					if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
					} else {
						roll_sp = (_manual.y * _parameters.man_roll_max)
								+ _parameters.rollsp_offset_rad;
					}

					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}
					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}
					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
					}

				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
 					/*
					 * Velocity should be controlled and manual is enabled.
					*/
					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}
					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}
					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
					}
				} else {
					/*
					 * Scale down roll and pitch as the setpoints are radians
					 * and a typical remote can only do around 45 degrees, the mapping is
					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
					 *
					 * With this mapping the stick angle is a 1:1 representation of
					 * the commanded attitude.
					 *
					 * The trim gets subtracted here from the manual setpoint to get
					 * the intended attitude setpoint. Later, after the rate control step the
					 * trim is added again to the control signal.
					 */
					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
					pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
					/* allow manual control of rudder deflection */
					yaw_manual = _manual.r;
					throttle_sp = _manual.z;

					/*
					 * in manual mode no external source should / does emit attitude setpoints.
					 * emit the manual setpoint here to allow attitude controller tuning
					 * in attitude control mode.
					 */
					struct vehicle_attitude_setpoint_s att_sp;
					att_sp.timestamp = hrt_absolute_time();
					att_sp.roll_body = roll_sp;
					att_sp.pitch_body = pitch_sp;
					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
					att_sp.thrust = throttle_sp;

					/* lazily publish the setpoint only once available */
					if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
						/* publish the attitude setpoint */
						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);

					} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
						/* advertise and publish */
						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
					}
				}

				/* If the aircraft is on ground reset the integrators */
				if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
					_roll_ctrl.reset_integrator();
					_pitch_ctrl.reset_integrator();
					_yaw_ctrl.reset_integrator();
				}

				/* Prepare speed_body_u and speed_body_w */
				float speed_body_u = 0.0f;
				float speed_body_v = 0.0f;
				float speed_body_w = 0.0f;
				if(_att.R_valid) 	{
					speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
					speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
					speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
				} else	{
					if (_debug && loop_counter % 10 == 0) {
						warnx("Did not get a valid R\n");
					}
				}

				/* Prepare data for attitude controllers */
				struct ECL_ControlData control_input = {};
				control_input.roll = _att.roll;
				control_input.pitch = _att.pitch;
				control_input.yaw = _att.yaw;
				control_input.roll_rate = _att.rollspeed;
				control_input.pitch_rate = _att.pitchspeed;
				control_input.yaw_rate = _att.yawspeed;
				control_input.speed_body_u = speed_body_u;
				control_input.speed_body_v = speed_body_v;
				control_input.speed_body_w = speed_body_w;
				control_input.acc_body_x = _accel.x;
				control_input.acc_body_y = _accel.y;
				control_input.acc_body_z = _accel.z;
				control_input.roll_setpoint = roll_sp;
				control_input.pitch_setpoint = pitch_sp;
				control_input.airspeed_min = _parameters.airspeed_min;
				control_input.airspeed_max = _parameters.airspeed_max;
				control_input.airspeed = airspeed;
				control_input.scaler = airspeed_scaling;
				control_input.lock_integrator = lock_integrator;

				/* Run attitude controllers */
				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
					_roll_ctrl.control_attitude(control_input);
					_pitch_ctrl.control_attitude(control_input);
					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude

					/* Update input data for rate controllers */
					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
					control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
					control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();

					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
					float roll_u = _roll_ctrl.control_bodyrate(control_input);
					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
					if (!isfinite(roll_u)) {
						_roll_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);

						if (_debug && loop_counter % 10 == 0) {
							warnx("roll_u %.4f", (double)roll_u);
						}
					}

					float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
					if (!isfinite(pitch_u)) {
						_pitch_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);
						if (_debug && loop_counter % 10 == 0) {
							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
								" airspeed %.4f, airspeed_scaling %.4f,"
								" roll_sp %.4f, pitch_sp %.4f,"
								" _roll_ctrl.get_desired_rate() %.4f,"
								" _pitch_ctrl.get_desired_rate() %.4f"
								" att_sp.roll_body %.4f",
								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
								(double)airspeed, (double)airspeed_scaling,
								(double)roll_sp, (double)pitch_sp,
								(double)_roll_ctrl.get_desired_rate(),
								(double)_pitch_ctrl.get_desired_rate(),
								(double)_att_sp.roll_body);
						}
					}

					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;

					/* add in manual rudder control */
					_actuators.control[2] += yaw_manual;
					if (!isfinite(yaw_u)) {
						_yaw_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);
						if (_debug && loop_counter % 10 == 0) {
							warnx("yaw_u %.4f", (double)yaw_u);
						}
					}

					/* throttle passed through if it is finite and if no engine failure was
					 * detected */
					_actuators.control[3] = (isfinite(throttle_sp) &&
							!(_vehicle_status.engine_failure ||
								_vehicle_status.engine_failure_cmd)) ?
						throttle_sp : 0.0f;
					if (!isfinite(throttle_sp)) {
						if (_debug && loop_counter % 10 == 0) {
							warnx("throttle_sp %.4f", (double)throttle_sp);
						}
					}
				} else {
					perf_count(_nonfinite_input_perf);
					if (_debug && loop_counter % 10 == 0) {
						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
					}
				}

				/*
				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
				 * only once available
				 */
				_rates_sp.roll = _roll_ctrl.get_desired_rate();
				_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
				_rates_sp.yaw = _yaw_ctrl.get_desired_rate();

				_rates_sp.timestamp = hrt_absolute_time();

				if (_rate_sp_pub > 0) {
					/* publish the attitude rates setpoint */
					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
				} else if (_rates_sp_id) {
					/* advertise the attitude rates setpoint */
					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
				}

			} else {
				/* manual/direct control */
				_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll;
				_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch;
				_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw;
				_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
			}

			_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
			_actuators.control[5] = _manual.aux1;
			_actuators.control[6] = _manual.aux2;
			_actuators.control[7] = _manual.aux3;

			/* lazily publish the setpoint only once available */
			_actuators.timestamp = hrt_absolute_time();
			_actuators.timestamp_sample = _att.timestamp;
			_actuators_airframe.timestamp = hrt_absolute_time();
			_actuators_airframe.timestamp_sample = _att.timestamp;

			/* Only publish if any of the proper modes are enabled */
			if(_vcontrol_mode.flag_control_rates_enabled ||
			   _vcontrol_mode.flag_control_attitude_enabled ||
			   _vcontrol_mode.flag_control_manual_enabled)
			{
				/* publish the actuator controls */
				if (_actuators_0_pub > 0) {
					orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
				} else if (_actuators_id) {
					_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
				}

				if (_actuators_2_pub > 0) {
					/* publish the actuator controls*/
					orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);

				} else {
					/* advertise and publish */
					_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
				}
			}
		}

		loop_counter++;
		perf_end(_loop_perf);
	}

	warnx("exiting.\n");

	_control_task = -1;
	_task_running = false;
	_exit(0);
}
/****************************************************************************
 * main
 ****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
	int mavlink_fd;
	mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);

	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
	float z_est[2] = { 0.0f, 0.0f };	// pos, vel

	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
	float R_gps[3][3];					// rotation matrix for GPS correction moment
	memset(est_buf, 0, sizeof(est_buf));
	memset(R_buf, 0, sizeof(R_buf));
	memset(R_gps, 0, sizeof(R_gps));
	int buf_ptr = 0;

	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation

	float eph = max_eph_epv;
	float epv = 1.0f;

	float eph_flow = 1.0f;

	float eph_vision = 0.2f;
	float epv_vision = 0.2f;

	float eph_mocap = 0.05f;
	float epv_mocap = 0.05f;

	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
	memset(x_est_prev, 0, sizeof(x_est_prev));
	memset(y_est_prev, 0, sizeof(y_est_prev));
	memset(z_est_prev, 0, sizeof(z_est_prev));

	int baro_init_cnt = 0;
	int baro_init_num = 200;
	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
	float surface_offset = 0.0f;	// ground level offset from reference altitude
	float surface_offset_rate = 0.0f;	// surface offset change rate

	hrt_abstime accel_timestamp = 0;
	hrt_abstime baro_timestamp = 0;

	bool ref_inited = false;
	hrt_abstime ref_init_start = 0;
	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
	struct map_projection_reference_s ref;
	memset(&ref, 0, sizeof(ref));
	hrt_abstime home_timestamp = 0;

	uint16_t accel_updates = 0;
	uint16_t baro_updates = 0;
	uint16_t gps_updates = 0;
	uint16_t attitude_updates = 0;
	uint16_t flow_updates = 0;
	uint16_t vision_updates = 0;
	uint16_t mocap_updates = 0;

	hrt_abstime updates_counter_start = hrt_absolute_time();
	hrt_abstime pub_last = hrt_absolute_time();

	hrt_abstime t_prev = 0;

	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
	float acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
	float corr_baro = 0.0f;		// D
	float corr_gps[3][2] = {
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};
	float w_gps_xy = 1.0f;
	float w_gps_z = 1.0f;

	float corr_vision[3][2] = {
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};

	float corr_mocap[3][1] = {
		{ 0.0f },		// N (pos)
		{ 0.0f },		// E (pos)
		{ 0.0f },		// D (pos)
	};

	float corr_sonar = 0.0f;
	float corr_sonar_filtered = 0.0f;

	float corr_flow[] = { 0.0f, 0.0f };	// N E
	float w_flow = 0.0f;

	float sonar_prev = 0.0f;
	//hrt_abstime flow_prev = 0;			// time of last flow measurement
	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)

	bool gps_valid = false;			// GPS is valid
	bool sonar_valid = false;		// sonar is valid
	bool flow_valid = false;		// flow is valid
	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
	bool vision_valid = false;		// vision is valid
	bool mocap_valid = false;		// mocap is valid

	/* declare and safely initialize all structs */
	struct actuator_controls_s actuator;
	memset(&actuator, 0, sizeof(actuator));
	struct actuator_armed_s armed;
	memset(&armed, 0, sizeof(armed));
	struct sensor_combined_s sensor;
	memset(&sensor, 0, sizeof(sensor));
	struct vehicle_gps_position_s gps;
	memset(&gps, 0, sizeof(gps));
	struct home_position_s home;
	memset(&home, 0, sizeof(home));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_local_position_s local_pos;
	memset(&local_pos, 0, sizeof(local_pos));
	struct optical_flow_s flow;
	memset(&flow, 0, sizeof(flow));
	struct vision_position_estimate_s vision;
	memset(&vision, 0, sizeof(vision));
	struct att_pos_mocap_s mocap;
	memset(&mocap, 0, sizeof(mocap));
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));

	/* subscribe */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
	int home_position_sub = orb_subscribe(ORB_ID(home_position));

	/* advertise */
	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
	orb_advert_t vehicle_global_position_pub = NULL;

	struct position_estimator_inav_params params;
	struct position_estimator_inav_param_handles pos_inav_param_handles;
	/* initialize parameter handles */
	inav_parameters_init(&pos_inav_param_handles);

	/* first parameters read at start up */
	struct parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
	/* first parameters update */
	inav_parameters_update(&pos_inav_param_handles, &params);

	px4_pollfd_struct_t fds_init[1] = {
		{ .fd = sensor_combined_sub, .events = POLLIN },
	};

	/* wait for initial baro value */
	bool wait_baro = true;

	thread_running = true;

	while (wait_baro && !thread_should_exit) {
		int ret = px4_poll(fds_init, 1, 1000);

		if (ret < 0) {
			/* poll error */
			mavlink_log_info(mavlink_fd, "[inav] poll error on init");

		} else if (ret > 0) {
			if (fds_init[0].revents & POLLIN) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
					baro_timestamp = sensor.baro_timestamp[0];

					/* mean calculation over several measurements */
					if (baro_init_cnt < baro_init_num) {
						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
							baro_offset += sensor.baro_alt_meter[0];
							baro_init_cnt++;
						}

					} else {
						wait_baro = false;
						baro_offset /= (float) baro_init_cnt;
						warnx("baro offset: %d m", (int)baro_offset);
						mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset);
						local_pos.z_valid = true;
						local_pos.v_z_valid = true;
					}
				}
			}
		}
	}

	/* main loop */
	px4_pollfd_struct_t fds[1] = {
		{ .fd = vehicle_attitude_sub, .events = POLLIN },
	};

	while (!thread_should_exit) {
		int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
		hrt_abstime t = hrt_absolute_time();

		if (ret < 0) {
			/* poll error */
			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
			continue;

		} else if (ret > 0) {
			/* act on attitude updates */

			/* vehicle attitude */
			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
			attitude_updates++;

			bool updated;

			/* parameter update */
			orb_check(parameter_update_sub, &updated);

			if (updated) {
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
				inav_parameters_update(&pos_inav_param_handles, &params);
			}

			/* actuator */
			orb_check(actuator_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
			}

			/* armed */
			orb_check(armed_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
			}

			/* sensor combined */
			orb_check(sensor_combined_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
					if (att.R_valid) {
						/* correct accel bias */
						sensor.accelerometer_m_s2[0] -= acc_bias[0];
						sensor.accelerometer_m_s2[1] -= acc_bias[1];
						sensor.accelerometer_m_s2[2] -= acc_bias[2];

						/* transform acceleration vector from body frame to NED frame */
						for (int i = 0; i < 3; i++) {
							acc[i] = 0.0f;

							for (int j = 0; j < 3; j++) {
								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
							}
						}

						acc[2] += CONSTANTS_ONE_G;

					} else {
						memset(acc, 0, sizeof(acc));
					}

					accel_timestamp = sensor.accelerometer_timestamp[0];
					accel_updates++;
				}

				if (sensor.baro_timestamp[0] != baro_timestamp) {
					corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
					baro_timestamp = sensor.baro_timestamp[0];
					baro_updates++;
				}
			}

			/* optical flow */
			orb_check(optical_flow_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);

				/* calculate time from previous update */
//				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
//				flow_prev = flow.flow_timestamp;

				if ((flow.ground_distance_m > 0.31f) &&
					(flow.ground_distance_m < 4.0f) &&
					(PX4_R(att.R, 2, 2) > 0.7f) &&
					(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {

					sonar_time = t;
					sonar_prev = flow.ground_distance_m;
					corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
					corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;

					if (fabsf(corr_sonar) > params.sonar_err) {
						/* correction is too large: spike or new ground level? */
						if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
							/* spike detected, ignore */
							corr_sonar = 0.0f;
							sonar_valid = false;

						} else {
							/* new ground level */
							surface_offset -= corr_sonar;
							surface_offset_rate = 0.0f;
							corr_sonar = 0.0f;
							corr_sonar_filtered = 0.0f;
							sonar_valid_time = t;
							sonar_valid = true;
							local_pos.surface_bottom_timestamp = t;
							mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
						}

					} else {
						/* correction is ok, use it */
						sonar_valid_time = t;
						sonar_valid = true;
					}
				}

				float flow_q = flow.quality / 255.0f;
				float dist_bottom = - z_est[0] - surface_offset;

				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
					/* distance to surface */
					float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
					/* check if flow if too large for accurate measurements */
					/* calculate estimated velocity in body frame */
					float body_v_est[2] = { 0.0f, 0.0f };

					for (int i = 0; i < 2; i++) {
						body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
					}

					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;

					/* convert raw flow to angular flow (rad/s) */
					float flow_ang[2];
					//todo check direction of x und y axis
					flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
					flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
					/* flow measurements vector */
					float flow_m[3];
					flow_m[0] = -flow_ang[0] * flow_dist;
					flow_m[1] = -flow_ang[1] * flow_dist;
					flow_m[2] = z_est[1];
					/* velocity in NED */
					float flow_v[2] = { 0.0f, 0.0f };

					/* project measurements vector to NED basis, skip Z component */
					for (int i = 0; i < 2; i++) {
						for (int j = 0; j < 3; j++) {
							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
						}
					}

					/* velocity correction */
					corr_flow[0] = flow_v[0] - x_est[1];
					corr_flow[1] = flow_v[1] - y_est[1];
					/* adjust correction weight */
					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);

					/* if flow is not accurate, reduce weight for it */
					// TODO make this more fuzzy
					if (!flow_accurate) {
						w_flow *= 0.05f;
					}

					/* under ideal conditions, on 1m distance assume EPH = 10cm */
					eph_flow = 0.1f / w_flow;

					flow_valid = true;

				} else {
					w_flow = 0.0f;
					flow_valid = false;
				}

				flow_updates++;
			}

			/* home position */
			orb_check(home_position_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(home_position), home_position_sub, &home);

				if (home.timestamp != home_timestamp) {
					home_timestamp = home.timestamp;

					double est_lat, est_lon;
					float est_alt;

					if (ref_inited) {
						/* calculate current estimated position in global frame */
						est_alt = local_pos.ref_alt - local_pos.z;
						map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
					}

					/* update reference */
					map_projection_init(&ref, home.lat, home.lon);

					/* update baro offset */
					baro_offset += home.alt - local_pos.ref_alt;

					local_pos.ref_lat = home.lat;
					local_pos.ref_lon = home.lon;
					local_pos.ref_alt = home.alt;
					local_pos.ref_timestamp = home.timestamp;

					if (ref_inited) {
						/* reproject position estimate with new reference */
						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
						z_est[0] = -(est_alt - local_pos.ref_alt);
					}

					ref_inited = true;
				}
			}


			/* check no vision circuit breaker is set */
			if (params.no_vision != CBRK_NO_VISION_KEY) {
				/* vehicle vision position */
				orb_check(vision_position_estimate_sub, &updated);

				if (updated) {
					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);

					static float last_vision_x = 0.0f;
					static float last_vision_y = 0.0f;
					static float last_vision_z = 0.0f;

					/* reset position estimate on first vision update */
					if (!vision_valid) {
						x_est[0] = vision.x;
						x_est[1] = vision.vx;
						y_est[0] = vision.y;
						y_est[1] = vision.vy;
						/* only reset the z estimate if the z weight parameter is not zero */
						if (params.w_z_vision_p > MIN_VALID_W)
						{
							z_est[0] = vision.z;
							z_est[1] = vision.vz;
						}

						vision_valid = true;

						last_vision_x = vision.x;
						last_vision_y = vision.y;
						last_vision_z = vision.z;

						warnx("VISION estimate valid");
						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
					}

					/* calculate correction for position */
					corr_vision[0][0] = vision.x - x_est[0];
					corr_vision[1][0] = vision.y - y_est[0];
					corr_vision[2][0] = vision.z - z_est[0];

					static hrt_abstime last_vision_time = 0;

					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
					last_vision_time = vision.timestamp_boot;

					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
						vision.vx = (vision.x - last_vision_x) / vision_dt;
						vision.vy = (vision.y - last_vision_y) / vision_dt;
						vision.vz = (vision.z - last_vision_z) / vision_dt;

						last_vision_x = vision.x;
						last_vision_y = vision.y;
						last_vision_z = vision.z;

						/* calculate correction for velocity */
						corr_vision[0][1] = vision.vx - x_est[1];
						corr_vision[1][1] = vision.vy - y_est[1];
						corr_vision[2][1] = vision.vz - z_est[1];
					} else {
						/* assume zero motion */
						corr_vision[0][1] = 0.0f - x_est[1];
						corr_vision[1][1] = 0.0f - y_est[1];
						corr_vision[2][1] = 0.0f - z_est[1];
					}

					vision_updates++;
				}
			}

			/* vehicle mocap position */
			orb_check(att_pos_mocap_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);

				/* reset position estimate on first mocap update */
				if (!mocap_valid) {
					x_est[0] = mocap.x;
					y_est[0] = mocap.y;
					z_est[0] = mocap.z;

					mocap_valid = true;

					warnx("MOCAP data valid");
					mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
				}

				/* calculate correction for position */
				corr_mocap[0][0] = mocap.x - x_est[0];
				corr_mocap[1][0] = mocap.y - y_est[0];
				corr_mocap[2][0] = mocap.z - z_est[0];

				mocap_updates++;
			}

			/* vehicle GPS position */
			orb_check(vehicle_gps_position_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);

				bool reset_est = false;

				/* hysteresis for GPS quality */
				if (gps_valid) {
					if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
						gps_valid = false;
						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
					}

				} else {
					if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
						gps_valid = true;
						reset_est = true;
						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
					}
				}

				if (gps_valid) {
					double lat = gps.lat * 1e-7;
					double lon = gps.lon * 1e-7;
					float alt = gps.alt * 1e-3;

					/* initialize reference position if needed */
					if (!ref_inited) {
						if (ref_init_start == 0) {
							ref_init_start = t;

						} else if (t > ref_init_start + ref_init_delay) {
							ref_inited = true;

							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
							x_est[0] = 0.0f;
							x_est[1] = gps.vel_n_m_s;
							y_est[0] = 0.0f;
							y_est[1] = gps.vel_e_m_s;

							local_pos.ref_lat = lat;
							local_pos.ref_lon = lon;
							local_pos.ref_alt = alt + z_est[0];
							local_pos.ref_timestamp = t;

							/* initialize projection */
							map_projection_init(&ref, lat, lon);
							// XXX replace this print
							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
						}
					}

					if (ref_inited) {
						/* project GPS lat lon to plane */
						float gps_proj[2];
						map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));

						/* reset position estimate when GPS becomes good */
						if (reset_est) {
							x_est[0] = gps_proj[0];
							x_est[1] = gps.vel_n_m_s;
							y_est[0] = gps_proj[1];
							y_est[1] = gps.vel_e_m_s;
						}

						/* calculate index of estimated values in buffer */
						int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
						if (est_i < 0) {
							est_i += EST_BUF_SIZE;
						}

						/* calculate correction for position */
						corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
						corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
						corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];

						/* calculate correction for velocity */
						if (gps.vel_ned_valid) {
							corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
							corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
							corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];

						} else {
							corr_gps[0][1] = 0.0f;
							corr_gps[1][1] = 0.0f;
							corr_gps[2][1] = 0.0f;
						}

						/* save rotation matrix at this moment */
						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));

						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
					}

				} else {
					/* no GPS lock */
					memset(corr_gps, 0, sizeof(corr_gps));
					ref_init_start = 0;
				}

				gps_updates++;
			}
		}

		/* check for timeout on FLOW topic */
		if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
			flow_valid = false;
			sonar_valid = false;
			warnx("FLOW timeout");
			mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
		}

		/* check for timeout on GPS topic */
		if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
			gps_valid = false;
			warnx("GPS timeout");
			mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
		}

		/* check for timeout on vision topic */
		if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
			vision_valid = false;
			warnx("VISION timeout");
			mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
		}

		/* check for timeout on mocap topic */
		if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
			mocap_valid = false;
			warnx("MOCAP timeout");
			mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
		}

		/* check for sonar measurement timeout */
		if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
			corr_sonar = 0.0f;
			sonar_valid = false;
		}

		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
		t_prev = t;

		/* increase EPH/EPV on each step */
		if (eph < max_eph_epv) {
			eph *= 1.0f + dt;
		}
		if (epv < max_eph_epv) {
			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
		}

		/* use GPS if it's valid and reference position initialized */
		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
		/* use VISION if it's valid and has a valid weight parameter */
		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
		/* use MOCAP if it's valid and has a valid weight parameter */
		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
		/* use flow if it's valid and (accurate or no GPS available) */
		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);

		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;

		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);

		if (dist_bottom_valid) {
			/* surface distance prediction */
			surface_offset += surface_offset_rate * dt;

			/* surface distance correction */
			if (sonar_valid) {
				surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
				surface_offset -= corr_sonar * params.w_z_sonar * dt;
			}
		}

		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
		float w_z_gps_v = params.w_z_gps_v * w_gps_z;

		float w_xy_vision_p = params.w_xy_vision_p;
		float w_xy_vision_v = params.w_xy_vision_v;
		float w_z_vision_p = params.w_z_vision_p;

		float w_mocap_p = params.w_mocap_p;

		/* reduce GPS weight if optical flow is good */
		if (use_flow && flow_accurate) {
			w_xy_gps_p *= params.w_gps_flow;
			w_xy_gps_v *= params.w_gps_flow;
		}

		/* baro offset correction */
		if (use_gps_z) {
			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
			baro_offset += offs_corr;
			corr_baro += offs_corr;
		}

		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };

		if (use_gps_xy) {
			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
		}

		if (use_gps_z) {
			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
		}

		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += R_gps[j][i] * accel_bias_corr[j];
			}

			if (isfinite(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;
			}
		}

		/* accelerometer bias correction for VISION (use buffered rotation matrix) */
		accel_bias_corr[0] = 0.0f;
		accel_bias_corr[1] = 0.0f;
		accel_bias_corr[2] = 0.0f;

		if (use_vision_xy) {
			accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
			accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
			accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
			accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
		}

		if (use_vision_z) {
			accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
		}

		/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
		accel_bias_corr[0] = 0.0f;
		accel_bias_corr[1] = 0.0f;
		accel_bias_corr[2] = 0.0f;

		if (use_mocap) {
			accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
			accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
			accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
		}

		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
			}

			if (isfinite(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;
			}
		}

		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
		accel_bias_corr[0] = 0.0f;
		accel_bias_corr[1] = 0.0f;
		accel_bias_corr[2] = 0.0f;

		if (use_flow) {
			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
		}

		accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;

		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
			}

			if (isfinite(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;
			}
		}

		/* inertial filter prediction for altitude */
		inertial_filter_predict(dt, z_est, acc[2]);

		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
			memcpy(z_est, z_est_prev, sizeof(z_est));
		}

		/* inertial filter correction for altitude */
		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);

		if (use_gps_z) {
			epv = fminf(epv, gps.epv);

			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
		}

		if (use_vision_z) {
			epv = fminf(epv, epv_vision);
			inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
		}

		if (use_mocap) {
			epv = fminf(epv, epv_mocap);
			inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
		}

		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);										
			memcpy(z_est, z_est_prev, sizeof(z_est));
			memset(corr_gps, 0, sizeof(corr_gps));
			memset(corr_vision, 0, sizeof(corr_vision));
			memset(corr_mocap, 0, sizeof(corr_mocap));
			corr_baro = 0;

		} else {
			memcpy(z_est_prev, z_est, sizeof(z_est));
		}

		if (can_estimate_xy) {
			/* inertial filter prediction for position */
			inertial_filter_predict(dt, x_est, acc[0]);
			inertial_filter_predict(dt, y_est, acc[1]);

			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
				memcpy(x_est, x_est_prev, sizeof(x_est));
				memcpy(y_est, y_est_prev, sizeof(y_est));
			}

			/* inertial filter correction for position */
			if (use_flow) {
				eph = fminf(eph, eph_flow);

				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
			}

			if (use_gps_xy) {
				eph = fminf(eph, gps.eph);

				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);

				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
				}
			}

			if (use_vision_xy) {
				eph = fminf(eph, eph_vision);

				inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
				inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);

				if (w_xy_vision_v > MIN_VALID_W) {
					inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
					inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
				}
			}

			if (use_mocap) {
				eph = fminf(eph, eph_mocap);

				inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
				inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
			}

			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
				memcpy(x_est, x_est_prev, sizeof(x_est));
				memcpy(y_est, y_est_prev, sizeof(y_est));
				memset(corr_gps, 0, sizeof(corr_gps));
				memset(corr_vision, 0, sizeof(corr_vision));
				memset(corr_mocap, 0, sizeof(corr_mocap));
				memset(corr_flow, 0, sizeof(corr_flow));

			} else {
				memcpy(x_est_prev, x_est, sizeof(x_est));
				memcpy(y_est_prev, y_est, sizeof(y_est));
			}
		} else {
			/* gradually reset xy velocity estimates */
			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
		}

		if (inav_verbose_mode) {
			/* print updates rate */
			if (t > updates_counter_start + updates_counter_len) {
				float updates_dt = (t - updates_counter_start) * 0.000001f;
				warnx(
					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
					(double)(accel_updates / updates_dt),
					(double)(baro_updates / updates_dt),
					(double)(gps_updates / updates_dt),
					(double)(attitude_updates / updates_dt),
					(double)(flow_updates / updates_dt),
					(double)(vision_updates / updates_dt),
					(double)(mocap_updates / updates_dt));
				updates_counter_start = t;
				accel_updates = 0;
				baro_updates = 0;
				gps_updates = 0;
				attitude_updates = 0;
				flow_updates = 0;
				vision_updates = 0;
				mocap_updates = 0;
			}
		}

		if (t > pub_last + PUB_INTERVAL) {
			pub_last = t;

			/* push current estimate to buffer */
			est_buf[buf_ptr][0][0] = x_est[0];
			est_buf[buf_ptr][0][1] = x_est[1];
			est_buf[buf_ptr][1][0] = y_est[0];
			est_buf[buf_ptr][1][1] = y_est[1];
			est_buf[buf_ptr][2][0] = z_est[0];
			est_buf[buf_ptr][2][1] = z_est[1];

			/* push current rotation matrix to buffer */
			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));

			buf_ptr++;
			if (buf_ptr >= EST_BUF_SIZE) {
				buf_ptr = 0;
			}

			/* publish local position */
			local_pos.xy_valid = can_estimate_xy;
			local_pos.v_xy_valid = can_estimate_xy;
			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
			local_pos.z_global = local_pos.z_valid && use_gps_z;
			local_pos.x = x_est[0];
			local_pos.vx = x_est[1];
			local_pos.y = y_est[0];
			local_pos.vy = y_est[1];
			local_pos.z = z_est[0];
			local_pos.vz = z_est[1];
			local_pos.yaw = att.yaw;
			local_pos.dist_bottom_valid = dist_bottom_valid;
			local_pos.eph = eph;
			local_pos.epv = epv;

			if (local_pos.dist_bottom_valid) {
				local_pos.dist_bottom = -z_est[0] - surface_offset;
				local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
			}

			local_pos.timestamp = t;

			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);

			if (local_pos.xy_global && local_pos.z_global) {
				/* publish global position */
				global_pos.timestamp = t;
				global_pos.time_utc_usec = gps.time_utc_usec;

				double est_lat, est_lon;
				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);

				global_pos.lat = est_lat;
				global_pos.lon = est_lon;
				global_pos.alt = local_pos.ref_alt - local_pos.z;

				global_pos.vel_n = local_pos.vx;
				global_pos.vel_e = local_pos.vy;
				global_pos.vel_d = local_pos.vz;

				global_pos.yaw = local_pos.yaw;

				global_pos.eph = eph;
				global_pos.epv = epv;

				if (vehicle_global_position_pub == NULL) {
					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

				} else {
					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
				}
			}
		}
	}

	warnx("stopped");
	mavlink_log_info(mavlink_fd, "[inav] stopped");
	thread_running = false;
	return 0;
}
示例#3
0
void
Navigator::gps_position_update()
{
	orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}
示例#4
0
void task_main(int argc, char *argv[])
{
	_is_running = true;

	if (uart_initialize(_device) < 0) {
		PX4_ERR("Failed to initialize UART.");
		return;
	}

	// Subscribe for orb topics
	_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	// Start disarmed
	_armed.armed = false;
	_armed.prearmed = false;

	// Set up poll topic
	px4_pollfd_struct_t fds[1];
	fds[0].fd     = _controls_sub;
	fds[0].events = POLLIN;
	/* Don't limit poll intervall for now, 250 Hz should be fine. */
	//orb_set_interval(_controls_sub, 10);

	// Set up mixer
	if (initialize_mixer(MIXER_FILENAME) < 0) {
		PX4_ERR("Mixer initialization failed.");
		return;
	}

	pwm_limit_init(&_pwm_limit);

	// TODO XXX: this is needed otherwise we crash in the callback context.
	_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc);

	// Main loop
	while (!_task_should_exit) {

		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);

		/* Timed out, do a periodic check for _task_should_exit. */
		if (pret == 0) {
			continue;
		}

		/* This is undesirable but not much we can do. */
		if (pret < 0) {
			PX4_WARN("poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);

			_outputs.timestamp = _controls.timestamp;

			/* do mixing */
			_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);

			/* disable unused ports by setting their output to NaN */
			for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
				_outputs.output[i] = NAN;
			}

			const uint16_t reverse_mask = 0;
			uint16_t disarmed_pwm[4];
			uint16_t min_pwm[4];
			uint16_t max_pwm[4];

			for (unsigned int i = 0; i < 4; i++) {
				disarmed_pwm[i] = _pwm_disarmed;
				min_pwm[i] = _pwm_min;
				max_pwm[i] = _pwm_max;
			}

			uint16_t pwm[4];

			// TODO FIXME: pre-armed seems broken
			pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask,
				       disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);


			send_outputs_mavlink(pwm, 4);

			if (_outputs_pub != nullptr) {
				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);

			} else {
				_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
			}
		}

		bool updated;
		orb_check(_armed_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
		}
	}

	uart_deinitialize();
	orb_unsubscribe(_controls_sub);
	orb_unsubscribe(_armed_sub);

	_is_running = false;

}
void *multirotor_position_control_thread_main()
{
	/* welcome user */
	fprintf (stdout, "Multirotor position controller started\n");
	fflush(stdout);

	int i;
	bool_t updated;
	bool_t reset_mission_sp = 0 /* false */;
	bool_t global_pos_sp_valid = 0 /* false */;
	bool_t reset_man_sp_z = 1 /* true */;
	bool_t reset_man_sp_xy = 1 /* true */;
	bool_t reset_int_z = 1 /* true */;
	bool_t reset_int_z_manual = 0 /* false */;
	bool_t reset_int_xy = 1 /* true */;
	bool_t was_armed = 0 /* false */;
	bool_t reset_auto_sp_xy = 1 /* true */;
	bool_t reset_auto_sp_z = 1 /* true */;
	bool_t reset_takeoff_sp = 1 /* true */;

	absolute_time t_prev = 0;
	const float alt_ctl_dz = 0.2f;
	const float pos_ctl_dz = 0.05f;

	float i_limit;	/* use integral_limit_out = tilt_max / 2 */
	float ref_alt = 0.0f;
	absolute_time ref_alt_t = 0;
	absolute_time local_ref_timestamp = 0;

	PID_t xy_pos_pids[2];
	PID_t xy_vel_pids[2];
	PID_t z_pos_pid;
	thrust_pid_t z_vel_pid;

	float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };


	/* structures */
	struct parameter_update_s ps;
	struct vehicle_control_flags_s control_flags;
	memset(&control_flags, 0, sizeof(control_flags));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_attitude_setpoint_s att_sp;
	memset(&att_sp, 0, sizeof(att_sp));
	struct manual_control_setpoint_s manual;
	memset(&manual, 0, sizeof(manual));
	struct vehicle_local_position_s local_pos;
	memset(&local_pos, 0, sizeof(local_pos));
	struct vehicle_local_position_setpoint_s local_pos_sp;
	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
	struct vehicle_global_position_setpoint_s global_pos_sp;
	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
	struct vehicle_global_velocity_setpoint_s global_vel_sp;
	memset(&global_vel_sp, 0, sizeof(global_vel_sp));


	/* subscribe to attitude, motor setpoints and system state */
	orb_subscr_t param_sub = orb_subscribe(ORB_ID(parameter_update));
	if (param_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to parameter_update topic\n");
		exit(-1);
	}

	orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags));
	if (control_flags_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to vehicle_control_flags topic\n");
		exit(-1);
	}


	orb_subscr_t att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	if (att_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude topic\n");
		exit(-1);
	}


	orb_subscr_t att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	if (att_sp_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude_setpoint topic\n");
		exit(-1);
	}


	orb_subscr_t manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	if (manual_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to manual_control_setpoint topic\n");
		exit(-1);
	}


	orb_subscr_t local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	if (local_pos_sp_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position_setpoint topic\n");
		exit(-1);
	}


	orb_subscr_t local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	if (local_pos_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position topic\n");
		exit(-1);
	}


	orb_subscr_t global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
	if (global_pos_sp_sub < 0)
	{
		fprintf (stderr, "Position controller thread failed to subscribe to vehicle_global_position_setpoint topic\n");
		exit(-1);
	}

	/* publish setpoint */
	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint));
	if (local_pos_sp_pub == -1)
	{
		fprintf (stderr, "Comunicator thread failed to advertise the vehicle_local_position_setpoint topic\n");
		exit (-1);
	}
	orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);

	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint));
	if (global_vel_sp_pub == -1)
	{
		fprintf (stderr, "Comunicator thread failed to advertise the vehicle_global_velocity_setpoint topic\n");
		exit (-1);
	}
	orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);

	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint));
	if (att_sp_pub == -1)
	{
		fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n");
		exit (-1);
	}
	orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);



	/* abort on a nonzero return value from the parameter init */
	if (multirotor_position_control_params_init() != 0) {
		/* parameter setup went wrong, abort */
		fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n");
		exit(-1);
	}

	for (i = 0; i < 2; i++) {
		pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f,
				multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
		pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i,
				multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
	}

	pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
			multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
	thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
			multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);


	while (!_shutdown_all_systems) {

		updated = orb_check (ORB_ID(parameter_update), param_sub);
		if (updated) {
			/* clear updated flag */
			orb_copy(ORB_ID(parameter_update), param_sub, &ps);

			/* update multirotor_position_control_parameters */
			multirotor_position_control_params_update();

			for (i = 0; i < 2; i++) {
				pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p,
						0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f);

				if (multirotor_position_control_parameters.xy_vel_i > 0.0f) {
					i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f;

				} else {
					i_limit = 0.0f;	// not used
				}

				pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p,
						multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max);
			}

			pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
					multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max);
			thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
					multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min);
		}

		updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub);
		if (updated) {
			orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags);
		}

		updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub);
		if (updated) {
			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
			global_pos_sp_valid = 1 /* true */;
			reset_mission_sp = 1 /* true */;
		}

		absolute_time t = get_absolute_time();
		float dt;

		if (t_prev != 0) {
			dt = (t - t_prev) * 0.000001f;

		} else {
			dt = 0.0f;
		}

		if (control_flags.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			reset_man_sp_z = 1 /* true */;
			reset_man_sp_xy = 1 /* true */;
			reset_auto_sp_z = 1 /* true */;
			reset_auto_sp_xy = 1 /* true */;
			reset_takeoff_sp = 1 /* true */;
			reset_int_z = 1 /* true */;
			reset_int_xy = 1 /* true */;
		}

		was_armed = control_flags.flag_armed;

		t_prev = t;

		if (control_flags.flag_control_altitude_enabled || control_flags.flag_control_velocity_enabled || control_flags.flag_control_position_enabled) {
			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);

			float z_sp_offs_max = multirotor_position_control_parameters.z_vel_max / multirotor_position_control_parameters.z_p * 2.0f;
			float xy_sp_offs_max = multirotor_position_control_parameters.xy_vel_max / multirotor_position_control_parameters.xy_p * 2.0f;
			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };

			if (control_flags.flag_control_manual_enabled) {
				/* manual control */
				/* check for reference point updates and correct setpoint */
				if (local_pos.ref_timestamp != ref_alt_t) {
					if (ref_alt_t != 0) {
						/* home alt changed, don't follow large ground level changes in manual flight */
						local_pos_sp.z += local_pos.ref_alt - ref_alt;
					}

					ref_alt_t = local_pos.ref_timestamp;
					ref_alt = local_pos.ref_alt;
					// TODO also correct XY setpoint
				}

				/* reset setpoints to current position if needed */
				if (control_flags.flag_control_altitude_enabled) {
					if (reset_man_sp_z) {
						reset_man_sp_z = 0 /* false */;
						local_pos_sp.z = local_pos.z;

						//mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
					}

					/* move altitude setpoint with throttle stick */
					float z_sp_ctl = scale_control(manual.thrust - 0.5f, 0.5f, alt_ctl_dz);

					if (z_sp_ctl != 0.0f) {
						sp_move_rate[2] = -z_sp_ctl * multirotor_position_control_parameters.z_vel_max;
						local_pos_sp.z += sp_move_rate[2] * dt;

						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
							local_pos_sp.z = local_pos.z + z_sp_offs_max;

						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
							local_pos_sp.z = local_pos.z - z_sp_offs_max;
						}
					}
				}

				if (control_flags.flag_control_position_enabled) {
					if (reset_man_sp_xy) {
						reset_man_sp_xy = 0 /* false */;
						local_pos_sp.x = local_pos.x;
						local_pos_sp.y = local_pos.y;
						pid_reset_integral(&xy_vel_pids[0]);
						pid_reset_integral(&xy_vel_pids[1]);

						//mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
					}

					/* move position setpoint with roll/pitch stick */
					float pos_pitch_sp_ctl = scale_control(-manual.pitch / multirotor_position_control_parameters.rc_scale_pitch, 1.0f, pos_ctl_dz);
					float pos_roll_sp_ctl = scale_control(manual.roll / multirotor_position_control_parameters.rc_scale_roll, 1.0f, pos_ctl_dz);

					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
						/* calculate direction and increment of control in NED frame */
						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * multirotor_position_control_parameters.xy_vel_max;
						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
						local_pos_sp.x += sp_move_rate[0] * dt;
						local_pos_sp.y += sp_move_rate[1] * dt;
						/* limit maximum setpoint from position offset and preserve direction
						 * fail safe, should not happen in normal operation */
						float pos_vec_x = local_pos_sp.x - local_pos.x;
						float pos_vec_y = local_pos_sp.y - local_pos.y;
						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;

						if (pos_vec_norm > 1.0f) {
							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
						}
					}
				}

				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
				local_pos_sp.yaw = att_sp.yaw_body;

				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
				reset_auto_sp_xy = !control_flags.flag_control_position_enabled;
				reset_auto_sp_z = !control_flags.flag_control_altitude_enabled;
				reset_takeoff_sp = 1 /* true */;

				/* force reprojection of global setpoint after manual mode */
				reset_mission_sp = 1 /* true */;

			} else if (control_flags.flag_control_auto_enabled) {
				/* AUTO mode, use global setpoint */
				if (control_flags.auto_state == navigation_state_auto_ready) {
					reset_auto_sp_xy = 1 /* true */;
					reset_auto_sp_z = 1 /* true */;

				} else if (control_flags.auto_state == navigation_state_auto_takeoff) {
					if (reset_takeoff_sp) {
						reset_takeoff_sp = 0 /* false */;
						local_pos_sp.x = local_pos.x;
						local_pos_sp.y = local_pos.y;
						local_pos_sp.z = - multirotor_position_control_parameters.takeoff_alt - multirotor_position_control_parameters.takeoff_gap;
						att_sp.yaw_body = att.yaw;

						//mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
					}

					reset_auto_sp_xy = 0 /* false */;
					reset_auto_sp_z = 1 /* true */;

				} else if (control_flags.auto_state == navigation_state_auto_rtl) {
					// TODO
					reset_auto_sp_xy = 1 /* true */;
					reset_auto_sp_z = 1 /* true */;

				} else if (control_flags.auto_state == navigation_state_auto_mission) {
					/* init local projection using local position ref */
					if (local_pos.ref_timestamp != local_ref_timestamp) {
						reset_mission_sp = 1 /* true */;
						local_ref_timestamp = local_pos.ref_timestamp;
						double lat_home = local_pos.ref_lat * 1e-7;
						double lon_home = local_pos.ref_lon * 1e-7;
						map_projection_init(lat_home, lon_home);

						//mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
					}

					if (reset_mission_sp) {
						reset_mission_sp = 0 /* false */;
						/* update global setpoint projection */

						if (global_pos_sp_valid) {
							/* global position setpoint valid, use it */
							double sp_lat = global_pos_sp.latitude * 1e-7;
							double sp_lon = global_pos_sp.longitude * 1e-7;
							/* project global setpoint to local setpoint */
							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));

							if (global_pos_sp.altitude_is_relative) {
								local_pos_sp.z = -global_pos_sp.altitude;

							} else {
								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
							}
							att_sp.yaw_body = global_pos_sp.yaw;

							//mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);

						} else {
							if (reset_auto_sp_xy) {
								reset_auto_sp_xy = 0 /* false */;
								/* local position setpoint is invalid,
								 * use current position as setpoint for loiter */
								local_pos_sp.x = local_pos.x;
								local_pos_sp.y = local_pos.y;
								local_pos_sp.yaw = att.yaw;
							}

							if (reset_auto_sp_z) {
								reset_auto_sp_z = 0 /* false */;
								local_pos_sp.z = local_pos.z;
							}

							//mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
						}
					}

					reset_auto_sp_xy = 1 /* true */;
					reset_auto_sp_z = 1 /* true */;
				}

				if (control_flags.auto_state != navigation_state_auto_takeoff) {
					reset_takeoff_sp = 1 /* true */;
				}

				if (control_flags.auto_state != navigation_state_auto_mission) {
					reset_mission_sp = 1 /* true */;
				}

				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
				local_pos_sp.yaw = att_sp.yaw_body;

				/* reset setpoints after AUTO mode */
				reset_man_sp_xy = 1 /* true */;
				reset_man_sp_z = 1 /* true */;

			} else {
				/* no control (failsafe), loiter or stay on ground */
				if (local_pos.landed) {
					/* landed: move setpoint down */
					/* in air: hold altitude */
					if (local_pos_sp.z < 5.0f) {
						/* set altitude setpoint to 5m under ground,
						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
						local_pos_sp.z = 5.0f;

						//mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
					}

					reset_man_sp_z = 1 /* true */;

				} else {
					/* in air: hold altitude */
					if (reset_man_sp_z) {
						reset_man_sp_z = 0 /* false */;
						local_pos_sp.z = local_pos.z;

						//mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
					}

					reset_auto_sp_z = 0 /* false */;
				}

				if (control_flags.flag_control_position_enabled) {
					if (reset_man_sp_xy) {
						reset_man_sp_xy = 0 /* false */;
						local_pos_sp.x = local_pos.x;
						local_pos_sp.y = local_pos.y;
						local_pos_sp.yaw = att.yaw;
						att_sp.yaw_body = att.yaw;

						//mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
					}

					reset_auto_sp_xy = 0 /* false */;
				}
			}

			/* publish local position setpoint */
			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);

			/* run position & altitude controllers, calculate velocity setpoint */
			if (control_flags.flag_control_altitude_enabled) {
				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];

			} else {
				reset_man_sp_z = 1 /* true */;
				global_vel_sp.vz = 0.0f;
			}

			if (control_flags.flag_control_position_enabled) {
				/* calculate velocity set point in NED frame */
				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];

				/* limit horizontal speed */
				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / multirotor_position_control_parameters.xy_vel_max;

				if (xy_vel_sp_norm > 1.0f) {
					global_vel_sp.vx /= xy_vel_sp_norm;
					global_vel_sp.vy /= xy_vel_sp_norm;
				}

			} else {
				reset_man_sp_xy = 1 /* true */;
				global_vel_sp.vx = 0.0f;
				global_vel_sp.vy = 0.0f;
			}

			//fprintf (stderr, "global_vel_sp.vx:%.3f\tglobal_vel_sp.vy:%.3f\tglobal_vel_sp.vz:%.3f\n", global_vel_sp.vx, global_vel_sp.vy, global_vel_sp.vz);

			/* publish new velocity setpoint */
			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
			// TODO subscribe to velocity setpoint if altitude/position control disabled

			if (control_flags.flag_control_climb_rate_enabled || control_flags.flag_control_velocity_enabled) {
				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */

				if (control_flags.flag_control_climb_rate_enabled) {
					if (reset_int_z) {
						reset_int_z = 0 /* false */;
						float i = multirotor_position_control_parameters.thr_min;

						if (reset_int_z_manual) {
							i = manual.thrust;

							if (i < multirotor_position_control_parameters.thr_min) {
								i = multirotor_position_control_parameters.thr_min;

							} else if (i > multirotor_position_control_parameters.thr_max) {
								i = multirotor_position_control_parameters.thr_max;
							}
						}

						thrust_pid_set_integral(&z_vel_pid, -i);

						//mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
					}

					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
					att_sp.thrust = -thrust_sp[2];

				} else {
					/* reset thrust integral when altitude control enabled */
					reset_int_z = 1 /* true */;
				}

				if (control_flags.flag_control_velocity_enabled) {
					/* calculate thrust set point in NED frame */
					if (reset_int_xy) {
						reset_int_xy = 0 /* false */;
						pid_reset_integral(&xy_vel_pids[0]);
						pid_reset_integral(&xy_vel_pids[1]);

						//mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
					}

					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);

					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
					/* limit horizontal part of thrust */
					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
					/* assuming that vertical component of thrust is g,
					 * horizontal component = g * tan(alpha) */
					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));

					if (tilt > multirotor_position_control_parameters.tilt_max) {
						tilt = multirotor_position_control_parameters.tilt_max;
					}

					/* convert direction to body frame */
					thrust_xy_dir -= att.yaw;
					/* calculate roll and pitch */
					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);

				} else {
					reset_int_xy = 1 /* true */;
				}

				att_sp.timestamp = get_absolute_time();
				//fprintf (stderr, "att_sp.roll:%.3f\tatt_sp.pitch:%.3f\tatt_sp.yaw:%.3f\tatt_sp.thrust:%.3f\n", att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);

				/* publish new attitude setpoint */
				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
			}

		} else {
			/* position controller disabled, reset setpoints */
			reset_man_sp_z = 1 /* true */;
			reset_man_sp_xy = 1 /* true */;
			reset_int_z = 1 /* true */;
			reset_int_xy = 1 /* true */;
			reset_mission_sp = 1 /* true */;
			reset_auto_sp_xy = 1 /* true */;
			reset_auto_sp_z = 1 /* true */;
		}

		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
		reset_int_z_manual = control_flags.flag_armed && control_flags.flag_control_manual_enabled && !control_flags.flag_control_climb_rate_enabled;

		/* run at approximately 50 Hz */
		usleep(20000);
	}


	/*
	 * do unsubscriptions
	 */
	orb_unsubscribe(ORB_ID(parameter_update), param_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_attitude), att_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, pthread_self());
	orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_local_position), local_pos_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, pthread_self());

	/*
	 * do unadvertises
	 */
	orb_unadvertise(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, pthread_self());
	orb_unadvertise(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, pthread_self());
	orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self());

	return 0;
}
void
MulticopterAttitudeControl::task_main()
{
	warnx("started");
	fflush(stdout);

	/*
	 * do subscriptions
	 */
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* initialize parameters cache */
	parameters_update();

	/* wakeup source: vehicle attitude */
	struct pollfd fds[1];

	fds[0].fd = _v_att_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		perf_begin(_loop_perf);

		/* run controller on attitude changes */
		if (fds[0].revents & POLLIN) {
			static uint64_t last_run = 0;
			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
			if (dt < 0.002f) {
				dt = 0.002f;

			} else if (dt > 0.02f) {
				dt = 0.02f;
			}

			/* copy attitude topic */
			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);

			/* check for updates in other topics */
			parameter_update_poll();
			vehicle_control_mode_poll();
			arming_status_poll();
			vehicle_manual_poll();

			if (_v_control_mode.flag_control_attitude_enabled) {
				control_attitude(dt);

				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub > 0) {
					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);

				} else {
					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
				}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_control_mode.flag_control_manual_enabled) {
					/* manual rates control - ACRO mode */
					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
					_thrust_sp = (_manual_control_sp.z + 1) / 2;

					/* reset yaw setpoint after ACRO */
					_reset_yaw_sp = true;

					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
					_v_rates_sp.timestamp = hrt_absolute_time();

					if (_v_rates_sp_pub > 0) {
						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);

					} else {
						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
					}

				} else {
					/* attitude controller disabled, poll rates setpoint topic */
					vehicle_rates_setpoint_poll();
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = _v_rates_sp.thrust;
				}
			}

			if (_v_control_mode.flag_control_rates_enabled) {
				control_attitude_rates(dt);

				/* publish actuator controls */
				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
				_actuators.timestamp = hrt_absolute_time();

				if (!_actuators_0_circuit_breaker_enabled) {
					if (_actuators_0_pub > 0) {
						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);

					} else {
						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
					}
				}
			}
		}

		perf_end(_loop_perf);
	}

	warnx("exit");

	_control_task = -1;
	_exit(0);
}
示例#7
0
int ardrone_interface_thread_main(int argc, char *argv[])
{
	thread_running = true;

	char *device = "/dev/ttyS1";

	/* welcome user */
	printf("[ardrone_interface] Control started, taking over motors\n");

	/* File descriptors */
	int gpios;

	char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";

	bool motor_test_mode = false;
	int test_motor = -1;

	/* read commandline arguments */
	for (int i = 0; i < argc && argv[i]; i++) {
		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
			motor_test_mode = true;
		}

		if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
			if (i+1 < argc) {
				int motor = atoi(argv[i+1]);
				if (motor > 0 && motor < 5) {
					test_motor = motor;
				} else {
					thread_running = false;
					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
				}
			} else {
				thread_running = false;
				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
			}
		}
		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
			if (argc > i + 1) {
				device = argv[i + 1];

			} else {
				thread_running = false;
				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
			}
		}
	}

	struct termios uart_config_original;

	if (motor_test_mode) {
		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
	}

	/* Led animation */
	int counter = 0;
	int led_counter = 0;

	/* declare and safely initialize all structs */
	struct vehicle_status_s state;
	memset(&state, 0, sizeof(state));
	struct actuator_controls_s actuator_controls;
	memset(&actuator_controls, 0, sizeof(actuator_controls));
	struct actuator_armed_s armed;
	armed.armed = false;

	/* subscribe to attitude, motor setpoints and system state */
	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	printf("[ardrone_interface] Motors initialized - ready.\n");
	fflush(stdout);

	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
	ardrone_write = ardrone_open_uart(device, &uart_config_original);

	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
	gpios = ar_multiplexing_init();

	if (ardrone_write < 0) {
		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
		thread_running = false;
		exit(ERROR);
	}

	/* initialize motors */
	if (OK != ar_init_motors(ardrone_write, gpios)) {
		close(ardrone_write);
		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
		thread_running = false;
		exit(ERROR);
	}

	ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);


	// XXX Re-done initialization to make sure it is accepted by the motors
	// XXX should be removed after more testing, but no harm

	/* close uarts */
	close(ardrone_write);

	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
	ardrone_write = ardrone_open_uart(device, &uart_config_original);

	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
	gpios = ar_multiplexing_init();

	if (ardrone_write < 0) {
		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
		thread_running = false;
		exit(ERROR);
	}

	/* initialize motors */
	if (OK != ar_init_motors(ardrone_write, gpios)) {
		close(ardrone_write);
		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
		thread_running = false;
		exit(ERROR);
	}

	while (!thread_should_exit) {

		if (motor_test_mode) {
			/* set motors to idle speed */
			if (test_motor > 0 && test_motor < 5) {
				int motors[4] = {0, 0, 0, 0};
				motors[test_motor - 1] = 10;
				ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
			} else {
				ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
			}

		} else {
			/* MAIN OPERATION MODE */

			/* get a local copy of the vehicle state */
			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
			/* get a local copy of the actuator controls */
			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
			
			/* for now only spin if armed and immediately shut down
			 * if in failsafe
			 */
			if (armed.armed && !armed.lockdown) {
				ardrone_mixing_and_output(ardrone_write, &actuator_controls);

			} else {
				/* Silently lock down motor speeds to zero */
				ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
			}
		}

		if (counter % 24 == 0) {
			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);

			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);

			if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);

			if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);

			if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);

			if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);

			if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);

			if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);

			if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);

			if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);

			if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);

			if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);

			led_counter++;

			if (led_counter == 12) led_counter = 0;
		}

		/* run at approximately 200 Hz */
		usleep(4500);

		counter++;
	}

	/* restore old UART config */
	int termios_state;

	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
	}

	printf("[ardrone_interface] Restored original UART config, exiting..\n");

	/* close uarts */
	close(ardrone_write);
	ar_multiplexing_deinit(gpios);

	fflush(stdout);

	thread_running = false;

	return OK;
}
示例#8
0
int uORBTest::UnitTest::test_multi2()
{

	test_note("Testing multi-topic 2 test (queue simulation)");
	//test: first subscribe, then advertise

	_thread_should_exit = false;
	const int num_instances = 3;
	int orb_data_fd[num_instances];
	int orb_data_next = 0;

	for (int i = 0; i < num_instances; ++i) {
//		PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
		orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
	}

	char *const args[1] = { nullptr };
	int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
					     SCHED_DEFAULT,
					     SCHED_PRIORITY_MAX - 5,
					     2000,
					     (px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
					     args);

	if (pubsub_task < 0) {
		return test_fail("failed launching task");
	}

	hrt_abstime last_time = 0;

	while (!_thread_should_exit) {

		bool updated = false;
		int orb_data_cur_fd = orb_data_fd[orb_data_next];
		orb_check(orb_data_cur_fd, &updated);

		if (updated) {
			struct orb_test_medium msg;
			orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);

// Relax timing requirement for Darwin CI system
#ifdef __PX4_DARWIN
			usleep(10000);
#else
			usleep(1000);
#endif

			if (last_time >= msg.time && last_time != 0) {
				return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
			}

			last_time = msg.time;

//			PX4_WARN("      got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
			orb_data_next = (orb_data_next + 1) % num_instances;
		}
	}

	for (int i = 0; i < num_instances; ++i) {
		orb_unsubscribe(orb_data_fd[i]);
	}

	return test_note("PASS multi-topic 2 test (queue simulation)");
}
示例#9
0
int uORBTest::UnitTest::test_multi_reversed()
{
	test_note("try multi-topic support subscribing before publishing");

	/* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */

	/* Subscribe first and advertise afterwards. */
	int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2);

	if (sfd2 < 0) {
		return test_fail("sub. id2: ret: %d", sfd2);
	}

	struct orb_test t {}, u {};

	t.val = 0;

	int instance2;

	_pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX);

	int instance3;

	_pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN);

	test_note("advertised");

	if (instance2 != 2) {
		return test_fail("mult. id2: %d", instance2);
	}

	if (instance3 != 3) {
		return test_fail("mult. id3: %d", instance3);
	}

	t.val = 204;

	if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) {
		return test_fail("mult. pub0 fail");
	}


	t.val = 304;

	if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) {
		return test_fail("mult. pub1 fail");
	}

	test_note("published");

	if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) {
		return test_fail("sub #2 copy failed: %d", errno);
	}

	if (u.val != 204) {
		return test_fail("sub #3 val. mismatch: %d", u.val);
	}

	int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3);

	if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) {
		return test_fail("sub #3 copy failed: %d", errno);
	}

	if (u.val != 304) {
		return test_fail("sub #3 val. mismatch: %d", u.val);
	}

	return test_note("PASS multi-topic reversed");
}
示例#10
0
int uORBTest::UnitTest::test_single()
{
	test_note("try single-topic support");

	struct orb_test t, u;
	int sfd;
	orb_advert_t ptopic;
	bool updated;

	t.val = 0;
	ptopic = orb_advertise(ORB_ID(orb_test), &t);

	if (ptopic == nullptr) {
		return test_fail("advertise failed: %d", errno);
	}

	test_note("publish handle %p", ptopic);
	sfd = orb_subscribe(ORB_ID(orb_test));

	if (sfd < 0) {
		return test_fail("subscribe failed: %d", errno);
	}

	test_note("subscribe fd %d", sfd);
	u.val = 1;

	if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) {
		return test_fail("copy(1) failed: %d", errno);
	}

	if (u.val != t.val) {
		return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
	}

	if (PX4_OK != orb_check(sfd, &updated)) {
		return test_fail("check(1) failed");
	}

	if (updated) {
		return test_fail("spurious updated flag");
	}

	t.val = 2;
	test_note("try publish");

	if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) {
		return test_fail("publish failed");
	}

	if (PX4_OK != orb_check(sfd, &updated)) {
		return test_fail("check(2) failed");
	}

	if (!updated) {
		return test_fail("missing updated flag");
	}

	if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) {
		return test_fail("copy(2) failed: %d", errno);
	}

	if (u.val != t.val) {
		return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
	}

	orb_unsubscribe(sfd);

	int ret = orb_unadvertise(ptopic);

	if (ret != PX4_OK) {
		return test_fail("orb_unadvertise failed: %i", ret);
	}

	return test_note("PASS single-topic test");
}
示例#11
0
int uORBTest::UnitTest::test_multi()
{
	/* this routine tests the multi-topic support */
	test_note("try multi-topic support");

	struct orb_test t {}, u {};
	t.val = 0;
	int instance0;
	_pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);

	test_note("advertised");

	int instance1;
	_pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);

	if (instance0 != 0) {
		return test_fail("mult. id0: %d", instance0);
	}

	if (instance1 != 1) {
		return test_fail("mult. id1: %d", instance1);
	}

	t.val = 103;

	if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) {
		return test_fail("mult. pub0 fail");
	}

	test_note("published");

	t.val = 203;

	if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) {
		return test_fail("mult. pub1 fail");
	}

	/* subscribe to both topics and ensure valid data is received */
	int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);

	if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) {
		return test_fail("sub #0 copy failed: %d", errno);
	}

	if (u.val != 103) {
		return test_fail("sub #0 val. mismatch: %d", u.val);
	}

	int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);

	if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) {
		return test_fail("sub #1 copy failed: %d", errno);
	}

	if (u.val != 203) {
		return test_fail("sub #1 val. mismatch: %d", u.val);
	}

	/* test priorities */
	int prio;

	if (PX4_OK != orb_priority(sfd0, &prio)) {
		return test_fail("prio #0");
	}

	if (prio != ORB_PRIO_MAX) {
		return test_fail("prio: %d", prio);
	}

	if (PX4_OK != orb_priority(sfd1, &prio)) {
		return test_fail("prio #1");
	}

	if (prio != ORB_PRIO_MIN) {
		return test_fail("prio: %d", prio);
	}

	if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) {
		return test_fail("latency test failed");
	}

	orb_unsubscribe(sfd0);
	orb_unsubscribe(sfd1);

	return test_note("PASS multi-topic test");
}
static int
flow_position_control_thread_main(int argc, char *argv[])
{
	/* welcome user */
	thread_running = true;
	printf("[flow position control] starting\n");

	uint32_t counter = 0;
	const float time_scale = powf(10.0f,-6.0f);

	/* structures */
	struct vehicle_status_s vstatus;
	struct vehicle_attitude_s att;
	struct manual_control_setpoint_s manual;
	struct filtered_bottom_flow_s filtered_flow;
	struct vehicle_local_position_s local_pos;

	struct vehicle_bodyframe_speed_setpoint_s speed_sp;

	/* subscribe to attitude, motor setpoints and system state */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));

	orb_advert_t speed_sp_pub;
	bool speed_setpoint_adverted = false;

	/* parameters init*/
	struct flow_position_control_params params;
	struct flow_position_control_param_handles param_handles;
	parameters_init(&param_handles);
	parameters_update(&param_handles, &params);

	/* init flow sum setpoint */
	float flow_sp_sumx = 0.0f;
	float flow_sp_sumy = 0.0f;

	/* init yaw setpoint */
	float yaw_sp = 0.0f;

	/* init height setpoint */
	float height_sp = params.height_min;

	/* height controller states */
	bool start_phase = true;
	bool landing_initialized = false;
	float landing_thrust_start = 0.0f;

	/* states */
	float integrated_h_error = 0.0f;
	float last_local_pos_z = 0.0f;
	bool update_flow_sp_sumx = false;
	bool update_flow_sp_sumy = false;
	uint64_t last_time = 0.0f;
	float dt = 0.0f; // s


	/* register the perf counter */
	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");

	static bool sensors_ready = false;

	while (!thread_should_exit)
	{
		/* wait for first attitude msg to be sure all data are available */
		if (sensors_ready)
		{
			/* polling */
			struct pollfd fds[2] = {
				{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
				{ .fd = parameter_update_sub,   .events = POLLIN }

			};

			/* wait for a position update, check for exit condition every 500 ms */
			int ret = poll(fds, 2, 500);

			if (ret < 0)
			{
				/* poll error, count it in perf */
				perf_count(mc_err_perf);
			}
			else if (ret == 0)
			{
				/* no return value, ignore */
//				printf("[flow position control] no filtered flow updates\n");
			}
			else
			{
				/* parameter update available? */
				if (fds[1].revents & POLLIN)
				{
					/* read from param to clear updated flag */
					struct parameter_update_s update;
					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);

					parameters_update(&param_handles, &params);
					printf("[flow position control] parameters updated.\n");
				}

				/* only run controller if position/speed changed */
				if (fds[0].revents & POLLIN)
				{
					perf_begin(mc_loop_perf);

					/* get a local copy of the vehicle state */
					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
					/* get a local copy of manual setpoint */
					orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
					/* get a local copy of attitude */
					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
					/* get a local copy of filtered bottom flow */
					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
					/* get a local copy of local position */
					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);

					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
					{
						float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
						float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1

						/* calc dt */
						if(last_time == 0)
						{
							last_time = hrt_absolute_time();
							continue;
						}
						dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
						last_time = hrt_absolute_time();

						/* update flow sum setpoint */
						if (update_flow_sp_sumx)
						{
							flow_sp_sumx = filtered_flow.sumx;
							update_flow_sp_sumx = false;
						}
						if (update_flow_sp_sumy)
						{
							flow_sp_sumy = filtered_flow.sumy;
							update_flow_sp_sumy = false;
						}

						/* calc new bodyframe speed setpoints */
						float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
						float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d;
						float speed_limit_height_factor = height_sp; // the settings are for 1 meter

						/* overwrite with rc input if there is any */
						if(isfinite(manual_pitch) && isfinite(manual_roll))
						{
							if(fabsf(manual_pitch) > params.manual_threshold)
							{
								speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
								update_flow_sp_sumx = true;
							}

							if(fabsf(manual_roll) > params.manual_threshold)
							{
								speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
								update_flow_sp_sumy = true;
							}
						}

						/* limit speed setpoints */
						if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
								(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
						{
							speed_sp.vx = speed_body_x;
						}
						else
						{
							if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
								speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
							if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
								speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
						}

						if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
								(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
						{
							speed_sp.vy = speed_body_y;
						}
						else
						{
							if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
								speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
							if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
								speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
						}

						/* manual yaw change */
						if(isfinite(manual_yaw) && isfinite(manual.throttle))
						{
							if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
							{
								yaw_sp += manual_yaw * params.limit_yaw_step;

								/* modulo for rotation -pi +pi */
								if(yaw_sp < -M_PI_F)
									yaw_sp = yaw_sp + M_TWOPI_F;
								else if(yaw_sp > M_PI_F)
									yaw_sp = yaw_sp - M_TWOPI_F;
							}
						}

						/* forward yaw setpoint */
						speed_sp.yaw_sp = yaw_sp;


						/* manual height control
						 * 0-20%: thrust linear down
						 * 20%-40%: down
						 * 40%-60%: stabilize altitude
						 * 60-100%: up
						 */
						float thrust_control = 0.0f;

						if (isfinite(manual.throttle))
						{
							if (start_phase)
							{
								/* control start thrust with stick input */
								if (manual.throttle < 0.4f)
								{
									/* first 40% for up to feedforward */
									thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
								}
								else
								{
									/* second 60% for up to feedforward + 10% */
									thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
								}

								/* exit start phase if setpoint is reached */
								if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
								{
									start_phase = false;
									/* switch to stabilize */
									thrust_control = params.thrust_feedforward;
								}
							}
							else
							{
								if (manual.throttle < 0.2f)
								{
									/* landing initialization */
									if (!landing_initialized)
									{
										/* consider last thrust control to avoid steps */
										landing_thrust_start = speed_sp.thrust_sp;
										landing_initialized = true;
									}

									/* set current height as setpoint to avoid steps */
									if (-local_pos.z > params.height_min)
										height_sp = -local_pos.z;
									else
										height_sp = params.height_min;

									/* lower 20% stick range controls thrust down */
									thrust_control = manual.throttle / 0.2f * landing_thrust_start;

									/* assume ground position here */
									if (thrust_control < 0.1f)
									{
										/* reset integral if on ground */
										integrated_h_error = 0.0f;
										/* switch to start phase */
										start_phase = true;
										/* reset height setpoint */
										height_sp = params.height_min;
									}
								}
								else
								{
									/* stabilized mode */
									landing_initialized = false;

									/* calc new thrust with PID */
									float height_error = (local_pos.z - (-height_sp));

									/* update height setpoint if needed*/
									if (manual.throttle < 0.4f)
									{
										/* down */
										if (height_sp > params.height_min + params.height_rate &&
												fabsf(height_error) < params.limit_height_error)
											height_sp -= params.height_rate * dt;
									}

									if (manual.throttle > 0.6f)
									{
										/* up */
										if (height_sp < params.height_max &&
												fabsf(height_error) < params.limit_height_error)
											height_sp += params.height_rate * dt;
									}

									/* instead of speed limitation, limit height error (downwards) */
									if(height_error > params.limit_height_error)
										height_error = params.limit_height_error;
									else if(height_error < -params.limit_height_error)
										height_error = -params.limit_height_error;

									integrated_h_error = integrated_h_error + height_error;
									float integrated_thrust_addition = integrated_h_error * params.height_i;

									if(integrated_thrust_addition > params.limit_thrust_int)
										integrated_thrust_addition = params.limit_thrust_int;
									if(integrated_thrust_addition < -params.limit_thrust_int)
										integrated_thrust_addition = -params.limit_thrust_int;

									float height_speed = last_local_pos_z - local_pos.z;
									float thrust_diff = height_error * params.height_p - height_speed * params.height_d;

									thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;

									/* add attitude component
									 * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
									 */
//									// TODO problem with attitude
//									if (att.R_valid && att.R[2][2] > 0)
//										thrust_control = thrust_control / att.R[2][2];

									/* set thrust lower limit */
									if(thrust_control < params.limit_thrust_lower)
										thrust_control = params.limit_thrust_lower;
								}
							}

							/* set thrust upper limit */
							if(thrust_control > params.limit_thrust_upper)
								thrust_control = params.limit_thrust_upper;
						}
						/* store actual height for speed estimation */
						last_local_pos_z = local_pos.z;

						speed_sp.thrust_sp = thrust_control;
						speed_sp.timestamp = hrt_absolute_time();

						/* publish new speed setpoint */
						if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
						{

							if(speed_setpoint_adverted)
							{
								orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
							}
							else
							{
								speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
								speed_setpoint_adverted = true;
							}
						}
						else
						{
							warnx("NaN in flow position controller!");
						}
					}
					else
					{
						/* in manual or stabilized state just reset speed and flow sum setpoint */
						speed_sp.vx = 0.0f;
						speed_sp.vy = 0.0f;
						flow_sp_sumx = filtered_flow.sumx;
						flow_sp_sumy = filtered_flow.sumy;
						if(isfinite(att.yaw))
						{
							yaw_sp = att.yaw;
							speed_sp.yaw_sp = att.yaw;
						}
						if(isfinite(manual.throttle))
							speed_sp.thrust_sp = manual.throttle;
					}

					/* measure in what intervals the controller runs */
					perf_count(mc_interval_perf);
					perf_end(mc_loop_perf);
				}
			}

			counter++;
		}
示例#13
0
int do_gyro_calibration(int mavlink_fd)
{
	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
	mavlink_log_info(mavlink_fd, "don't move system");

	struct gyro_scale gyro_scale = {
		0.0f,
		1.0f,
		0.0f,
		1.0f,
		0.0f,
		1.0f,
	};

	int res = OK;

	/* reset all offsets to zero and all scales to one */
	int fd = open(GYRO_DEVICE_PATH, 0);
	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
	close(fd);

	if (res != OK) {
		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
	}

	if (res == OK) {
		/* determine gyro mean values */
		const unsigned calibration_count = 5000;
		unsigned calibration_counter = 0;
		unsigned poll_errcount = 0;

		/* subscribe to gyro sensor topic */
		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
		struct gyro_report gyro_report;

		while (calibration_counter < calibration_count) {
			/* wait blocking for new data */
			struct pollfd fds[1];
			fds[0].fd = sub_sensor_gyro;
			fds[0].events = POLLIN;

			int poll_ret = poll(fds, 1, 1000);

			if (poll_ret > 0) {
				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
				gyro_scale.x_offset += gyro_report.x;
				gyro_scale.y_offset += gyro_report.y;
				gyro_scale.z_offset += gyro_report.z;
				calibration_counter++;

				if (calibration_counter % (calibration_count / 20) == 0)
					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);

			} else {
				poll_errcount++;
			}

			if (poll_errcount > 1000) {
				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
				res = ERROR;
				break;
			}
		}

		close(sub_sensor_gyro);

		gyro_scale.x_offset /= calibration_count;
		gyro_scale.y_offset /= calibration_count;
		gyro_scale.z_offset /= calibration_count;
	}

	if (res == OK) {
		/* check offsets */
		if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
			res = ERROR;
		}
	}

	if (res == OK) {
		/* set offset parameters to new values */
		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
			res = ERROR;
		}
	}

#if 0
	/* beep on offset calibration end */
	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
	tune_neutral();

	/* scale calibration */
	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */

	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");

	/* apply new offsets */
	fd = open(GYRO_DEVICE_PATH, 0);

	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
		warn("WARNING: failed to apply new offsets for gyro");

	close(fd);


	unsigned rotations_count = 30;
	float gyro_integral = 0.0f;
	float baseline_integral = 0.0f;

	// XXX change to mag topic
	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);

	float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);

	if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;

	if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;


	uint64_t last_time = hrt_absolute_time();
	uint64_t start_time = hrt_absolute_time();

	while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {

		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
		    && (hrt_absolute_time() - start_time > 5 * 1e6)) {
			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
			close(sub_sensor_combined);
			return OK;
		}

		/* wait blocking for new data */
		struct pollfd fds[1];
		fds[0].fd = sub_sensor_combined;
		fds[0].events = POLLIN;

		int poll_ret = poll(fds, 1, 1000);

		if (poll_ret) {

			float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
			last_time = hrt_absolute_time();

			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);

			// XXX this is just a proof of concept and needs world / body
			// transformation and more

			//math::Vector2f magNav(raw.magnetometer_ga);

			// calculate error between estimate and measurement
			// apply declination correction for true heading as well.
			//float mag = -atan2f(magNav(1),magNav(0));
			float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);

			if (mag > M_PI_F) mag -= 2 * M_PI_F;

			if (mag < -M_PI_F) mag += 2 * M_PI_F;

			float diff = mag - mag_last;

			if (diff > M_PI_F) diff -= 2 * M_PI_F;

			if (diff < -M_PI_F) diff += 2 * M_PI_F;

			baseline_integral += diff;
			mag_last = mag;
			// Jump through some timing scale hoops to avoid
			// operating near the 1e6/1e8 max sane resolution of float.
			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;

//			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);

			// } else if (poll_ret == 0) {
			// 	/* any poll failure for 1s is a reason to abort */
			// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
			// 	return;
		}
	}

	float gyro_scale = baseline_integral / gyro_integral;

	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);


	if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
		mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
		close(sub_sensor_gyro);
		mavlink_log_critical(mavlink_fd, "gyro calibration failed");
		return ERROR;
	}

	/* beep on calibration end */
	mavlink_log_info(mavlink_fd, "gyro scale calibration done");
	tune_neutral();

#endif

	if (res == OK) {
		/* set scale parameters to new values */
		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
			res = ERROR;
		}
	}

	if (res == OK) {
		/* apply new scaling and offsets */
		fd = open(GYRO_DEVICE_PATH, 0);
		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
		close(fd);

		if (res != OK) {
			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
		}
	}

	if (res == OK) {
		/* auto-save to EEPROM */
		res = param_save_default();

		if (res != OK) {
			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
		}
	}

	if (res == OK) {
		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);

	} else {
		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
	}

	return res;
}
示例#14
0
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
	calibrate_return result = calibrate_return_ok;
	
	unsigned int calibration_counter_side;

	mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
	
	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);

	/*
	 * Detect if the system is rotating.
	 *
	 * We're detecting this as a general rotation on any axis, not necessary on the one we
	 * asked the user for. This is because we really just need two roughly orthogonal axes
	 * for a good result, so we're not constraining the user more than we have to.
	 */

	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
	hrt_abstime last_gyro = 0;
	float gyro_x_integral = 0.0f;
	float gyro_y_integral = 0.0f;
	float gyro_z_integral = 0.0f;

	const float gyro_int_thresh_rad = 0.5f;

	int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));

	while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
		fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
		fabsf(gyro_z_integral) < gyro_int_thresh_rad) {

		/* abort on request */
		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			px4_close(sub_gyro);
			return result;
		}

		/* abort with timeout */
		if (hrt_absolute_time() > detection_deadline) {
			result = calibrate_return_error;
			warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
			mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
			break;
		}

		/* Wait clocking for new data on all gyro */
		px4_pollfd_struct_t fds[1];
		fds[0].fd = sub_gyro;
		fds[0].events = POLLIN;
		size_t fd_count = 1;

		int poll_ret = px4_poll(fds, fd_count, 1000);

		if (poll_ret > 0) {
			struct gyro_report gyro;
			orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);

			/* ensure we have a valid first timestamp */
			if (last_gyro > 0) {

				/* integrate */
				float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
				gyro_x_integral += gyro.x * delta_t;
				gyro_y_integral += gyro.y * delta_t;
				gyro_z_integral += gyro.z * delta_t;
			}

			last_gyro = gyro.timestamp;
		}
	}

	px4_close(sub_gyro);
	
	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
	unsigned poll_errcount = 0;
	
	calibration_counter_side = 0;
	
	while (hrt_absolute_time() < calibration_deadline &&
	       calibration_counter_side < worker_data->calibration_points_perside) {
		
		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
			result = calibrate_return_cancelled;
			break;
		}
		
		// Wait clocking for new data on all mags
		px4_pollfd_struct_t fds[max_mags];
		size_t fd_count = 0;
		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
			if (worker_data->sub_mag[cur_mag] >= 0) {
				fds[fd_count].fd = worker_data->sub_mag[cur_mag];
				fds[fd_count].events = POLLIN;
				fd_count++;
			}
		}
		int poll_ret = px4_poll(fds, fd_count, 1000);
		
		if (poll_ret > 0) {

			int prev_count[max_mags];
			bool rejected = false;

			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {

				prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];

				if (worker_data->sub_mag[cur_mag] >= 0) {
					struct mag_report mag;

					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);

					// Check if this measurement is good to go in
					rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
						worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
						worker_data->calibration_counter_total[cur_mag],
						calibration_sides * worker_data->calibration_points_perside);
					
					worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
					worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
					worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
					worker_data->calibration_counter_total[cur_mag]++;
				}
			}

			// Keep calibration of all mags in lockstep
			if (rejected) {
				// Reset counts, since one of the mags rejected the measurement
				for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
					worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
				}
			} else {
				calibration_counter_side++;

				// Progress indicator for side
				mavlink_and_console_log_info(worker_data->mavlink_fd,
							     "[cal] %s side calibration: progress <%u>",
							     detect_orientation_str(orientation),
							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
			}
		} else {
			poll_errcount++;
		}
		
		if (poll_errcount > worker_data->calibration_points_perside * 3) {
			result = calibrate_return_error;
			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
			break;
		}
	}
	
	if (result == calibrate_return_ok) {
		mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
		
		worker_data->done_count++;
		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
	}
	
	return result;
}
示例#15
0
void
Navigator::params_update()
{
	parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
}
示例#16
0
int uORBTest::UnitTest::test_queue()
{
	test_note("Testing orb queuing");

	struct orb_test_medium t, u;
	int sfd;
	orb_advert_t ptopic;
	bool updated;

	sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));

	if (sfd < 0) {
		return test_fail("subscribe failed: %d", errno);
	}


	const int queue_size = 11;
	t.val = 0;
	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);

	if (ptopic == nullptr) {
		return test_fail("advertise failed: %d", errno);
	}

	orb_check(sfd, &updated);

	if (!updated) {
		return test_fail("update flag not set");
	}

	if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) {
		return test_fail("copy(1) failed: %d", errno);
	}

	if (u.val != t.val) {
		return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
	}

	orb_check(sfd, &updated);

	if (updated) {
		return test_fail("spurious updated flag");
	}

#define CHECK_UPDATED(element) \
	orb_check(sfd, &updated); \
	if (!updated) { \
		return test_fail("update flag not set, element %i", element); \
	}
#define CHECK_NOT_UPDATED(element) \
	orb_check(sfd, &updated); \
	if (updated) { \
		return test_fail("update flag set, element %i", element); \
	}
#define CHECK_COPY(i_got, i_correct) \
	orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \
	if (i_got != i_correct) { \
		return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \
	}

	//no messages in the queue anymore

	test_note("  Testing to write some elements...");

	for (int i = 0; i < queue_size - 2; ++i) {
		t.val = i;
		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
	}

	for (int i = 0; i < queue_size - 2; ++i) {
		CHECK_UPDATED(i);
		CHECK_COPY(u.val, i);
	}

	CHECK_NOT_UPDATED(queue_size);

	test_note("  Testing overflow...");
	int overflow_by = 3;

	for (int i = 0; i < queue_size + overflow_by; ++i) {
		t.val = i;
		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
	}

	for (int i = 0; i < queue_size; ++i) {
		CHECK_UPDATED(i);
		CHECK_COPY(u.val, i + overflow_by);
	}

	CHECK_NOT_UPDATED(queue_size);

	test_note("  Testing underflow...");

	for (int i = 0; i < queue_size; ++i) {
		CHECK_NOT_UPDATED(i);
		CHECK_COPY(u.val, queue_size + overflow_by - 1);
	}

	t.val = 943;
	orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
	CHECK_UPDATED(-1);
	CHECK_COPY(u.val, t.val);

#undef CHECK_COPY
#undef CHECK_UPDATED
#undef CHECK_NOT_UPDATED

	orb_unadvertise(ptopic);

	return test_note("PASS orb queuing");
}
示例#17
0
void
Navigator::task_main()
{
	bool have_geofence_position_data = false;

	/* Try to load the geofence:
	 * if /fs/microsd/etc/geofence.txt load from this file
	 * else clear geofence data in datamanager */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		warnx("Try to load geofence.txt");
		_geofence.loadFromFile(GEOFENCE_FILENAME);

	} else {
		if (_geofence.clearDm() != OK) {
			mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
		}
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_land_detected_update();
	vehicle_control_mode_update();
	global_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update(true);
	navigation_capabilities_update();
	params_update();

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[1] = {};

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;

	bool global_pos_available_once = false;

	while (!_task_should_exit) {

		/* wait for up to 200ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);

		if (pret == 0) {
			/* timed out - periodic check for _task_should_exit, etc. */
			if (global_pos_available_once) {
				PX4_WARN("navigator timed out");
			}
			continue;

		} else if (pret < 0) {
			/* this is undesirable but not much we can do - might want to flag unhappy status */
			PX4_WARN("nav: poll error %d, %d", pret, errno);
			continue;
		}

		global_pos_available_once = true;

		perf_begin(_loop_perf);

		bool updated;

		/* gps updated */
		orb_check(_gps_pos_sub, &updated);
		if (updated) {
			gps_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
				have_geofence_position_data = true;
			}
		}

		/* sensors combined updated */
		orb_check(_sensor_combined_sub, &updated);
		if (updated) {
			sensor_combined_update();
		}

		/* parameters updated */
		orb_check(_param_update_sub, &updated);
		if (updated) {
			params_update();
			updateParams();
		}

		/* vehicle control mode updated */
		orb_check(_control_mode_sub, &updated);
		if (updated) {
			vehicle_control_mode_update();
		}

		/* vehicle status updated */
		orb_check(_vstatus_sub, &updated);
		if (updated) {
			vehicle_status_update();
		}

		/* vehicle land detected updated */
		orb_check(_land_detected_sub, &updated);
		if (updated) {
			vehicle_land_detected_update();
		}

		/* navigation capabilities updated */
		orb_check(_capabilities_sub, &updated);
		if (updated) {
			navigation_capabilities_update();
		}

		/* home position updated */
		orb_check(_home_pos_sub, &updated);
		if (updated) {
			home_position_update();
		}

		orb_check(_vehicle_command_sub, &updated);
		if (updated) {
			vehicle_command_s cmd;
			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {

				struct position_setpoint_triplet_s *rep = get_reposition_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = NAN;
				rep->previous.lat = get_global_position()->lat;
				rep->previous.lon = get_global_position()->lon;
				rep->previous.alt = get_global_position()->alt;

				rep->current.loiter_radius = get_loiter_radius();
				rep->current.loiter_direction = 1;
				rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;

				// Go on and check which changes had been requested
				if (PX4_ISFINITE(cmd.param4)) {
					rep->current.yaw = cmd.param4;
				} else {
					rep->current.yaw = NAN;
				}

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
					rep->current.lat = cmd.param5 / (double)1e7;
					rep->current.lon = cmd.param6 / (double)1e7;
				} else {
					rep->current.lat = get_global_position()->lat;
					rep->current.lon = get_global_position()->lon;
				}

				if (PX4_ISFINITE(cmd.param7)) {
					rep->current.alt = cmd.param7;
				} else {
					rep->current.alt = get_global_position()->alt;
				}

				rep->previous.valid = true;
				rep->current.valid = true;
				rep->next.valid = false;
			}

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
				warnx("navigator: got pause/continue command");
			}
		}

		/* global position updated */
		if (fds[0].revents & POLLIN) {
			global_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
				have_geofence_position_data = true;
			}
		}

		/* Check geofence violation */
		static hrt_abstime last_geofence_check = 0;
		if (have_geofence_position_data &&
			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
			last_geofence_check = hrt_absolute_time();
			have_geofence_position_data = false;

			_geofence_result.geofence_action = _geofence.getGeofenceAction();
			if (!inside) {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = true;
				publish_geofence_result();

				/* Issue a warning about the geofence violation once */
				if (!_geofence_violation_warning_sent) {
					mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
					_geofence_violation_warning_sent = true;
				}
			} else {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = false;
				publish_geofence_result();
				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}
		}

		/* Do stuff according to navigation state set by commander */
		switch (_vstatus.nav_state) {
			case vehicle_status_s::NAVIGATION_STATE_MANUAL:
			case vehicle_status_s::NAVIGATION_STATE_ACRO:
			case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
			case vehicle_status_s::NAVIGATION_STATE_POSCTL:
			case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
			case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
				if (_nav_caps.abort_landing) {
					// pos controller aborted landing, requests loiter
					// above landing waypoint
					_navigation_mode = &_loiter;
					_pos_sp_triplet_published_invalid_once = false;
				} else {
					_pos_sp_triplet_published_invalid_once = false;
					_navigation_mode = &_mission;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_loiter;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_rcloss_act.get() == 1) {
					_navigation_mode = &_loiter;
				} else if (_param_rcloss_act.get() == 3) {
					_navigation_mode = &_land;
				} else if (_param_rcloss_act.get() == 4) {
					_navigation_mode = &_rcLoss;
				} else { /* if == 2 or unknown, RTL */
					_navigation_mode = &_rtl;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_rtl;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_takeoff;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_land;
				break;
			case vehicle_status_s::NAVIGATION_STATE_DESCEND:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_land;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
				/* Use complex data link loss mode only when enabled via param
				* otherwise use rtl */
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_datalinkloss_act.get() == 1) {
					_navigation_mode = &_loiter;
				} else if (_param_datalinkloss_act.get() == 3) {
					_navigation_mode = &_land;
				} else if (_param_datalinkloss_act.get() == 4) {
					_navigation_mode = &_dataLinkLoss;
				} else { /* if == 2 or unknown, RTL */
					_navigation_mode = &_rtl;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_engineFailure;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_gpsFailure;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_follow_target;
				break;
			default:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
		}

		/* iterate through navigation modes and set active/inactive for each */
		for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
		}

		/* if nothing is running, set position setpoint triplet invalid once */
		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
			_pos_sp_triplet_published_invalid_once = true;
			_pos_sp_triplet.previous.valid = false;
			_pos_sp_triplet.current.valid = false;
			_pos_sp_triplet.next.valid = false;
			_pos_sp_triplet_updated = true;
		}

		if (_pos_sp_triplet_updated) {
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		if (_mission_result_updated) {
			publish_mission_result();
			_mission_result_updated = false;
		}

		perf_end(_loop_perf);
	}
	warnx("exiting.");

	_navigator_task = -1;
	return;
}
示例#18
0
int uORBTest::UnitTest::pubsublatency_main()
{
	/* poll on test topic and output latency */
	float latency_integral = 0.0f;

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[3];

	int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0);
	int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
	int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);

	struct orb_test_large t;

	/* clear all ready flags */
	orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
	orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
	orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);

	fds[0].fd = test_multi_sub;
	fds[0].events = POLLIN;
	fds[1].fd = test_multi_sub_medium;
	fds[1].events = POLLIN;
	fds[2].fd = test_multi_sub_large;
	fds[2].events = POLLIN;

	const unsigned maxruns = 1000;
	unsigned timingsgroup = 0;
	int current_value = t.val;
	int num_missed = 0;

	// timings has to be on the heap to keep frame size below 2048 bytes
	unsigned *timings = new unsigned[maxruns];
	unsigned timing_min = 9999999, timing_max = 0;

	for (unsigned i = 0; i < maxruns; i++) {
		/* wait for up to 500ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
			timingsgroup = 0;

		} else if (fds[1].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
			timingsgroup = 1;

		} else if (fds[2].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
			timingsgroup = 2;
		}

		if (pret < 0) {
			PX4_ERR("poll error %d, %d", pret, errno);
			continue;
		}

		num_missed += t.val - current_value - 1;
		current_value = t.val;

		unsigned elt = (unsigned)hrt_elapsed_time(&t.time);
		latency_integral += elt;
		timings[i] = elt;

		if (elt > timing_max) {
			timing_max = elt;
		}

		if (elt < timing_min) {
			timing_min = elt;
		}
	}

	orb_unsubscribe(test_multi_sub);
	orb_unsubscribe(test_multi_sub_medium);
	orb_unsubscribe(test_multi_sub_large);

	if (pubsubtest_print) {
		char fname[32];
		sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
		FILE *f = fopen(fname, "w");

		if (f == nullptr) {
			PX4_ERR("Error opening file!");
			delete[] timings;
			return PX4_ERROR;
		}

		for (unsigned i = 0; i < maxruns; i++) {
			fprintf(f, "%u\n", timings[i]);
		}

		fclose(f);
	}


	float std_dev = 0.f;
	float mean = latency_integral / maxruns;

	for (unsigned i = 0; i < maxruns; i++) {
		float diff = (float)timings[i] - mean;
		std_dev += diff * diff;
	}

	delete[] timings;

	PX4_INFO("mean:    %8.4f us", static_cast<double>(mean));
	PX4_INFO("std dev: %8.4f us", static_cast<double>(sqrtf(std_dev / (maxruns - 1))));
	PX4_INFO("min:     %3i us", timing_min);
	PX4_INFO("max:     %3i us", timing_max);
	PX4_INFO("missed topic updates: %i", num_missed);

	pubsubtest_passed = true;

	if (static_cast<float>(latency_integral / maxruns) > 100.0f) {
		pubsubtest_res = PX4_ERROR;

	} else {
		pubsubtest_res = PX4_OK;
	}

	return pubsubtest_res;
}
示例#19
0
void FlightTasks::_updateCommand()
{
	// lazy subscription to command topic
	if (_sub_vehicle_command < 0) {
		_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
	}

	// check if there's any new command
	bool updated = false;
	orb_check(_sub_vehicle_command, &updated);

	if (!updated) {
		return;
	}

	// get command
	struct vehicle_command_s command;
	orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);

	// check what command it is
	FlightTaskIndex desired_task = switchVehicleCommand(command.command);

	if (desired_task == FlightTaskIndex::None) {
		// ignore all unkown commands
		return;
	}

	// switch to the commanded task
	int switch_result = switchTask(desired_task);
	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;

	// if we are in/switched to the desired task
	if (switch_result >= 0) {
		cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;

		// if the task is running apply parameters to it and see if it rejects
		if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;

			// if we just switched and parameters are not accepted, go to failsafe
			if (switch_result == 1) {
				switchTask(FlightTaskIndex::ManualPosition);
				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
			}
		}
	}

	// send back acknowledgment
	vehicle_command_ack_s command_ack = {};
	command_ack.command = command.command;
	command_ack.result = cmd_result;
	command_ack.result_param1 = switch_result;
	command_ack.target_system = command.source_system;
	command_ack.target_component = command.source_component;

	if (_pub_vehicle_command_ack == nullptr) {
		_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);

	} else {
		orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);

	}
}
示例#20
0
int uORBTest::UnitTest::test_queue_poll_notify()
{
	test_note("Testing orb queuing (poll & notify)");

	struct orb_test_medium t;
	int sfd;

	if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) {
		return test_fail("subscribe failed: %d", errno);
	}

	_thread_should_exit = false;

	char *const args[1] = { nullptr };
	int pubsub_task = px4_task_spawn_cmd("uorb_test_queue",
					     SCHED_DEFAULT,
					     SCHED_PRIORITY_MIN + 5,
					     1500,
					     (px4_main_t)&uORBTest::UnitTest::pub_test_queue_entry,
					     args);

	if (pubsub_task < 0) {
		return test_fail("failed launching task");
	}

	int next_expected_val = 0;
	px4_pollfd_struct_t fds[1];
	fds[0].fd = sfd;
	fds[0].events = POLLIN;

	while (!_thread_should_exit) {

		int poll_ret = px4_poll(fds, 1, 500);

		if (poll_ret == 0) {
			if (_thread_should_exit) {
				break;
			}

			return test_fail("poll timeout");

		} else if (poll_ret < 0) {
			return test_fail("poll error (%d, %d)", poll_ret, errno);
		}

		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t);

			if (next_expected_val != t.val) {
				return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val);
			}

			++next_expected_val;
		}
	}

	if (_num_messages_sent != next_expected_val) {
		return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)",
				 _num_messages_sent, next_expected_val);
	}

	return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val);
}
示例#21
0
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
	prctl(PR_SET_NAME, "uavcan fw srv", 0);

	Lock lock(_subnode_mutex);

	/*
	Copy any firmware bundled in the ROMFS to the appropriate location on the
	SD card, unless the user has copied other firmware for that device.
	*/
	unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);

	/* the subscribe call needs to happen in the same thread,
	 * so not in the constructor */
	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
	int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* Set up shared service clients */
	_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
	_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
	_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
	_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
	_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));

	_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
	memset(_param_counts, 0, sizeof(_param_counts));

	_esc_enumeration_active = false;
	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
	_esc_enumeration_index = 0;

	while (!_subnode_thread_should_exit) {

		if (_check_fw == true) {
			_check_fw = false;
			_node_info_retriever.invalidateAll();
		}

		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
		if (spin_res < 0) {
			warnx("node spin error %i", spin_res);
		}

		// Check for parameter requests (get/set/list)
		bool param_request_ready;
		orb_check(param_request_sub, &param_request_ready);

		if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			struct uavcan_parameter_request_s request;
			orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);

			if (_param_counts[request.node_id]) {
				/*
				 * We know how many parameters are exposed by this node, so
				 * process the request.
				 */
				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
					uavcan::protocol::param::GetSet::Request req;
					if (request.param_index >= 0) {
						req.index = request.param_index;
					} else {
						req.name = (char*)request.param_id;
					}

					int call_res = _param_getset_client.call(request.node_id, req);
					if (call_res < 0) {
						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
					} else {
						_param_in_progress = true;
						_param_index = request.param_index;
					}
				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
					uavcan::protocol::param::GetSet::Request req;
					if (request.param_index >= 0) {
						req.index = request.param_index;
					} else {
						req.name = (char*)request.param_id;
					}

					if (request.param_type == MAV_PARAM_TYPE_REAL32) {
						req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
					} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
						req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
					} else {
						req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
					}

					// Set the dirty bit for this node
					set_node_params_dirty(request.node_id);

					int call_res = _param_getset_client.call(request.node_id, req);
					if (call_res < 0) {
						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
					} else {
						_param_in_progress = true;
						_param_index = request.param_index;
					}
				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
					// This triggers the _param_list_in_progress case below.
					_param_index = 0;
					_param_list_in_progress = true;
					_param_list_node_id = request.node_id;
					_param_list_all_nodes = false;

					warnx("UAVCAN command bridge: starting component-specific param list");
				}
			} else if (request.node_id == MAV_COMP_ID_ALL) {
				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
					/*
					 * This triggers the _param_list_in_progress case below,
					 * but additionally iterates over all active nodes.
					 */
					_param_index = 0;
					_param_list_in_progress = true;
					_param_list_node_id = get_next_active_node_id(1);
					_param_list_all_nodes = true;

					warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);

					if (_param_counts[_param_list_node_id] == 0) {
						param_count(_param_list_node_id);
					}
				}
			} else {
				/*
				 * Need to know how many parameters this node has before we can
				 * continue; count them now and then process the request.
				 */
				param_count(request.node_id);
			}
		}

		// Handle parameter listing index/node ID advancement
		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			if (_param_index >= _param_counts[_param_list_node_id]) {
				warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
				// Reached the end of the current node's parameter set.
				_param_list_in_progress = false;

				if (_param_list_all_nodes) {
					// We're listing all parameters for all nodes -- get the next node ID
					uint8_t next_id = get_next_active_node_id(_param_list_node_id);
					if (next_id < 128) {
						_param_list_node_id = next_id;
						/*
						 * If there is a next node ID, check if that node's parameters
						 * have been counted before. If not, do it now.
						 */
						if (_param_counts[_param_list_node_id] == 0) {
							param_count(_param_list_node_id);
						}
						// Keep on listing.
						_param_index = 0;
						_param_list_in_progress = true;
						warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
					}
				}
			}
		}

		// Check if we're still listing, and need to get the next parameter
		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			// Ready to request the next value -- _param_index is incremented
			// after each successful fetch by cb_getset
			uavcan::protocol::param::GetSet::Request req;
			req.index = _param_index;

			int call_res = _param_getset_client.call(_param_list_node_id, req);
			if (call_res < 0) {
				_param_list_in_progress = false;
				warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
			} else {
				_param_in_progress = true;
			}
		}

		// Check for ESC enumeration commands
		bool cmd_ready;
		orb_check(cmd_sub, &cmd_ready);

		if (cmd_ready && !_cmd_in_progress) {
			struct vehicle_command_s cmd;
			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
				int command_id = static_cast<int>(cmd.param1 + 0.5f);
				int node_id = static_cast<int>(cmd.param2 + 0.5f);
				int call_res;

				warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);

				switch (command_id) {
				case 0:
				case 1: {
						_esc_enumeration_active = command_id;
						_esc_enumeration_index = 0;
						_esc_count = 0;
						uavcan::protocol::enumeration::Begin::Request req;
						req.parameter_name = "esc_index";
						req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
						call_res = _enumeration_client.call(get_next_active_node_id(1), req);
						if (call_res < 0) {
							warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
						}
						break;
					}
				default: {
						warnx("UAVCAN command bridge: unknown command ID %d", command_id);
						break;
					}
				}
			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
				int command_id = static_cast<int>(cmd.param1 + 0.5f);

				warnx("UAVCAN command bridge: received storage command ID %d", command_id);

				switch (command_id) {
				case 1: {
						// Param save request
						int node_id;
						node_id = get_next_dirty_node_id(1);
						if (node_id < 128) {
							_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
							param_opcode(node_id);
						}
						break;
					}
				case 2: {
						// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
						_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
						for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
							set_node_params_dirty(i);
						}
						param_opcode(get_next_dirty_node_id(1));
						break;
					}
				}
			}
		}

		// Shut down once armed
		// TODO (elsewhere): start up again once disarmed?
		bool updated;
		orb_check(armed_sub, &updated);
		if (updated) {
			struct actuator_armed_s armed;
			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);

			if (armed.armed && !armed.lockdown) {
				warnx("UAVCAN command bridge: system armed, exiting now.");
				break;
			}
		}
	}

	_subnode_thread_should_exit = false;

	warnx("exiting");
	return (pthread_addr_t) 0;
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
void
MulticopterAttitudeControl::task_main()
{

	/*
	 * do subscriptions
	 */
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));

	/* initialize parameters cache */
	parameters_update();

	/* wakeup source: vehicle attitude */
	px4_pollfd_struct_t fds[1];

	fds[0].fd = _ctrl_state_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("mc att ctrl: poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		perf_begin(_loop_perf);

		/* run controller on attitude changes */
		if (fds[0].revents & POLLIN) {
			static uint64_t last_run = 0;
			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
			if (dt < 0.002f) {
				dt = 0.002f;

			} else if (dt > 0.02f) {
				dt = 0.02f;
			}

			/* copy attitude and control state topics */
			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);

			/* check for updates in other topics */
			parameter_update_poll();
			vehicle_control_mode_poll();
			arming_status_poll();
			vehicle_manual_poll();
			vehicle_status_poll();
			vehicle_motor_limits_poll();

			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
			 * even bother running the attitude controllers */
			if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) {
				if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
				    fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
					_v_control_mode.flag_control_attitude_enabled = false;
				}
			}

			if (_v_control_mode.flag_control_attitude_enabled) {

				if (_ts_opt_recovery == nullptr) {
					// the  tailsitter recovery instance has not been created, thus, the vehicle
					// is not a tailsitter, do normal attitude control
					control_attitude(dt);

				} else {
					vehicle_attitude_setpoint_poll();
					_thrust_sp = _v_att_sp.thrust;
					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);

					/* limit rates */
					for (int i = 0; i < 3; i++) {
						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
					}
				}

				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub != nullptr) {
					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
				}

				//}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_control_mode.flag_control_manual_enabled) {
					/* manual rates control - ACRO mode */
					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
								    _manual_control_sp.r).emult(_params.acro_rate_max);
					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);

					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
					_v_rates_sp.timestamp = hrt_absolute_time();

					if (_v_rates_sp_pub != nullptr) {
						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

					} else if (_rates_sp_id) {
						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
					}

				} else {
					/* attitude controller disabled, poll rates setpoint topic */
					vehicle_rates_setpoint_poll();
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = _v_rates_sp.thrust;
				}
			}

			if (_v_control_mode.flag_control_rates_enabled) {
				control_attitude_rates(dt);

				/* publish actuator controls */
				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
				_actuators.timestamp = hrt_absolute_time();
				_actuators.timestamp_sample = _ctrl_state.timestamp;

				_controller_status.roll_rate_integ = _rates_int(0);
				_controller_status.pitch_rate_integ = _rates_int(1);
				_controller_status.yaw_rate_integ = _rates_int(2);
				_controller_status.timestamp = hrt_absolute_time();

				if (!_actuators_0_circuit_breaker_enabled) {
					if (_actuators_0_pub != nullptr) {

						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
						perf_end(_controller_latency_perf);

					} else if (_actuators_id) {
						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
					}

				}

				/* publish controller status */
				if (_controller_status_pub != nullptr) {
					orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);

				} else {
					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
				}
			}
		}

		perf_end(_loop_perf);
	}

	_control_task = -1;
	return;
}
/*
 * EKF Attitude Estimator main function.
 *
 * Estimates the attitude recursively once started.
 *
 * @param argc number of commandline arguments (plus command name)
 * @param argv strings containing the arguments
 */
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{

const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds

	float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
	float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
	float x_aposteriori_k[12];		/**< states */
	float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
				    }; /**< init: diagonal matrix with big values */

	float x_aposteriori[12];
	float P_aposteriori[144];

	/* output euler angles */
	float euler[3] = {0.0f, 0.0f, 0.0f};

	float Rot_matrix[9] = {1.f,  0,  0,
			      0,  1.f,  0,
			      0,  0,  1.f
			     };		/**< init: identity matrix */

	float debugOutput[4] = { 0.0f };
	int overloadcounter = 19;

	/* Initialize filter */
	AttitudeEKF_initialize();

	/* store start time to guard against too slow update rates */
	uint64_t last_run = hrt_absolute_time();

	struct sensor_combined_s raw;
	memset(&raw, 0, sizeof(raw));
	struct vehicle_gps_position_s gps;
	memset(&gps, 0, sizeof(gps));
	gps.eph = 100000;
	gps.epv = 100000;
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_control_mode_s control_mode;
	memset(&control_mode, 0, sizeof(control_mode));

	uint64_t last_data = 0;
	uint64_t last_measurement = 0;
	uint64_t last_vel_t = 0;

	/* current velocity */
	math::Vector<3> vel;
	vel.zero();
	/* previous velocity */
	math::Vector<3> vel_prev;
	vel_prev.zero();
	/* actual acceleration (by GPS velocity) in body frame */
	math::Vector<3> acc;
	acc.zero();
	/* rotation matrix */
	math::Matrix<3, 3> R;
	R.identity();

	/* subscribe to raw data */
	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
	orb_set_interval(sub_raw, 3);

	/* subscribe to GPS */
	int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));

	/* subscribe to GPS */
	int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));

	/* subscribe to param changes */
	int sub_params = orb_subscribe(ORB_ID(parameter_update));

	/* subscribe to control mode */
	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));

	/* subscribe to vision estimate */
	int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));

	/* advertise attitude */
	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);

	int loopcounter = 0;

	thread_running = true;

	/* keep track of sensor updates */
	uint64_t sensor_last_timestamp[3] = {0, 0, 0};

	struct attitude_estimator_ekf_params ekf_params;
	memset(&ekf_params, 0, sizeof(ekf_params));

	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };

	/* initialize parameter handles */
	parameters_init(&ekf_param_handles);

	bool initialized = false;

	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };

	/* magnetic declination, in radians */
	float mag_decl = 0.0f;

	/* rotation matrix for magnetic declination */
	math::Matrix<3, 3> R_decl;
	R_decl.identity();

	struct vision_position_estimate vision {};

	/* register the perf counter */
	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");

	/* Main loop*/
	while (!thread_should_exit) {

		struct pollfd fds[2];
		fds[0].fd = sub_raw;
		fds[0].events = POLLIN;
		fds[1].fd = sub_params;
		fds[1].events = POLLIN;
		int ret = poll(fds, 2, 1000);

		if (ret < 0) {
			/* XXX this is seriously bad - should be an emergency */
		} else if (ret == 0) {
			/* check if we're in HIL - not getting sensor data is fine then */
			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

			if (!control_mode.flag_system_hil_enabled) {
				warnx("WARNING: Not getting sensors - sensor app running?");
			}

		} else {

			/* only update parameters if they changed */
			if (fds[1].revents & POLLIN) {
				/* read from param to clear updated flag */
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), sub_params, &update);

				/* update parameters */
				parameters_update(&ekf_param_handles, &ekf_params);
			}

			/* only run filter if sensor values changed */
			if (fds[0].revents & POLLIN) {

				/* get latest measurements */
				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

				bool gps_updated;
				orb_check(sub_gps, &gps_updated);
				if (gps_updated) {
					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);

					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);
					}
				}

				bool global_pos_updated;
				orb_check(sub_global_pos, &global_pos_updated);
				if (global_pos_updated) {
					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
				}

				if (!initialized) {
					// XXX disabling init for now
					initialized = true;

					// gyro_offsets[0] += raw.gyro_rad_s[0];
					// gyro_offsets[1] += raw.gyro_rad_s[1];
					// gyro_offsets[2] += raw.gyro_rad_s[2];
					// offset_count++;

					// if (hrt_absolute_time() - start_time > 3000000LL) {
					// 	initialized = true;
					// 	gyro_offsets[0] /= offset_count;
					// 	gyro_offsets[1] /= offset_count;
					// 	gyro_offsets[2] /= offset_count;
					// }

				} else {

					perf_begin(ekf_loop_perf);

					/* Calculate data time difference in seconds */
					dt = (raw.timestamp - last_measurement) / 1000000.0f;
					last_measurement = raw.timestamp;
					uint8_t update_vect[3] = {0, 0, 0};

					/* Fill in gyro measurements */
					if (sensor_last_timestamp[0] != raw.timestamp) {
						update_vect[0] = 1;
						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
						sensor_last_timestamp[0] = raw.timestamp;
					}

					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];

					/* update accelerometer measurements */
					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
						update_vect[1] = 1;
						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
					}

					hrt_abstime vel_t = 0;
					bool vel_valid = false;
					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
						vel_valid = true;
						if (gps_updated) {
							vel_t = gps.timestamp_velocity;
							vel(0) = gps.vel_n_m_s;
							vel(1) = gps.vel_e_m_s;
							vel(2) = gps.vel_d_m_s;
						}

					} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
						vel_valid = true;
						if (global_pos_updated) {
							vel_t = global_pos.timestamp;
							vel(0) = global_pos.vel_n;
							vel(1) = global_pos.vel_e;
							vel(2) = global_pos.vel_d;
						}
					}

					if (vel_valid) {
						/* velocity is valid */
						if (vel_t != 0) {
							/* velocity updated */
							if (last_vel_t != 0 && vel_t != last_vel_t) {
								float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
								/* calculate acceleration in body frame */
								acc = R.transposed() * ((vel - vel_prev) / vel_dt);
							}
							last_vel_t = vel_t;
							vel_prev = vel;
						}

					} else {
						/* velocity is valid, reset acceleration */
						acc.zero();
						vel_prev.zero();
						last_vel_t = 0;
					}

					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);

					/* update magnetometer measurements */
					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
						update_vect[2] = 1;
						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
					}

					bool vision_updated = false;
					orb_check(vision_sub, &vision_updated);

					if (vision_updated) {
						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
					}

					if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {

						math::Quaternion q(vision.q);
						math::Matrix<3, 3> Rvis = q.to_dcm();

						math::Vector<3> v(1.0f, 0.0f, 0.4f);

						math::Vector<3> vn = Rvis * v;

						z_k[6] = vn(0);
						z_k[7] = vn(1);
						z_k[8] = vn(2);
					} else {
						z_k[6] = raw.magnetometer_ga[0];
						z_k[7] = raw.magnetometer_ga[1];
						z_k[8] = raw.magnetometer_ga[2];
					}

					uint64_t now = hrt_absolute_time();
					unsigned int time_elapsed = now - last_run;
					last_run = now;

					if (time_elapsed > loop_interval_alarm) {
						//TODO: add warning, cpu overload here
						// if (overloadcounter == 20) {
						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
						// 	overloadcounter = 0;
						// }

						overloadcounter++;
					}

					static bool const_initialized = false;

					/* initialize with good values once we have a reasonable dt estimate */
					if (!const_initialized && dt < 0.05f && dt > 0.001f) {
						dt = 0.005f;
						parameters_update(&ekf_param_handles, &ekf_params);

						/* update mag declination rotation matrix */
						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						} else {
							mag_decl = ekf_params.mag_decl;
						}

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);

						x_aposteriori_k[0] = z_k[0];
						x_aposteriori_k[1] = z_k[1];
						x_aposteriori_k[2] = z_k[2];
						x_aposteriori_k[3] = 0.0f;
						x_aposteriori_k[4] = 0.0f;
						x_aposteriori_k[5] = 0.0f;
						x_aposteriori_k[6] = z_k[3];
						x_aposteriori_k[7] = z_k[4];
						x_aposteriori_k[8] = z_k[5];
						x_aposteriori_k[9] = z_k[6];
						x_aposteriori_k[10] = z_k[7];
						x_aposteriori_k[11] = z_k[8];

						const_initialized = true;
					}

					/* do not execute the filter if not initialized */
					if (!const_initialized) {
						continue;
					}

					/* Call the estimator */
					AttitudeEKF(false, // approx_prediction
							(unsigned char)ekf_params.use_moment_inertia,
							update_vect,
							dt,
							z_k,
							ekf_params.q[0], // q_rotSpeed,
							ekf_params.q[1], // q_rotAcc
							ekf_params.q[2], // q_acc
							ekf_params.q[3], // q_mag
							ekf_params.r[0], // r_gyro
							ekf_params.r[1], // r_accel
							ekf_params.r[2], // r_mag
							ekf_params.moment_inertia_J,
							x_aposteriori,
							P_aposteriori,
							Rot_matrix,
							euler,
							debugOutput);

					/* swap values for next iteration, check for fatal inputs */
					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));

					} else {
						/* due to inputs or numerical failure the output is invalid, skip it */
						continue;
					}

					if (last_data > 0 && raw.timestamp - last_data > 30000)
						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);

					last_data = raw.timestamp;

					/* send out */
					att.timestamp = raw.timestamp;

					att.roll = euler[0];
					att.pitch = euler[1];
					att.yaw = euler[2] + mag_decl;

					att.rollspeed = x_aposteriori[0];
					att.pitchspeed = x_aposteriori[1];
					att.yawspeed = x_aposteriori[2];
					att.rollacc = x_aposteriori[3];
					att.pitchacc = x_aposteriori[4];
					att.yawacc = x_aposteriori[5];

					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);

					/* copy offsets */
					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

					/* magnetic declination */

					math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
					R = R_decl * R_body;
					math::Quaternion q;
					q.from_dcm(R);
					/* copy rotation matrix */
					memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
					memcpy(&att.q[0],&q.data[0],sizeof(att.q));
					att.R_valid = true;

					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
						// Broadcast
						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

					} else {
						warnx("NaN in roll/pitch/yaw estimate!");
					}

					perf_end(ekf_loop_perf);
				}
			}
		}

		loopcounter++;
	}

	thread_running = false;

	return 0;
}
示例#25
0
文件: sdlog2.c 项目: DMCKITT/Firmware
int sdlog2_thread_main(int argc, char *argv[])
{
	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	if (mavlink_fd < 0) {
		warnx("failed to open MAVLink log stream, start mavlink app first.");
	}

	/* log buffer size */
	int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;

	/* work around some stupidity in task_create's argv handling */
	argc -= 2;
	argv += 2;
	int ch;

	while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
		switch (ch) {
		case 'r': {
				unsigned long r = strtoul(optarg, NULL, 10);

				if (r == 0) {
					sleep_delay = 0;

				} else {
					sleep_delay = 1000000 / r;
				}
			}
			break;

		case 'b': {
				unsigned long s = strtoul(optarg, NULL, 10);

				if (s < 1) {
					s = 1;
				}

				log_buffer_size = 1024 * s;
			}
			break;

		case 'e':
			log_on_start = true;
			break;

		case 'a':
			log_when_armed = true;
			break;

		case '?':
			if (optopt == 'c') {
				warnx("Option -%c requires an argument.", optopt);

			} else if (isprint(optopt)) {
				warnx("Unknown option `-%c'.", optopt);

			} else {
				warnx("Unknown option character `\\x%x'.", optopt);
			}

		default:
			sdlog2_usage("unrecognized flag");
			errx(1, "exiting.");
		}
	}

	if (!file_exist(mountpoint)) {
		errx(1, "logging mount point %s not present, exiting.", mountpoint);
	}

	if (create_logfolder()) {
		errx(1, "unable to create logging folder, exiting.");
	}

	const char *converter_in = "/etc/logging/conv.zip";
	char *converter_out = malloc(120);
	sprintf(converter_out, "%s/conv.zip", folder_path);

	if (file_copy(converter_in, converter_out)) {
		errx(1, "unable to copy conversion scripts, exiting.");
	}

	free(converter_out);

	/* only print logging path, important to find log file later */
	warnx("logging to directory: %s", folder_path);

	/* initialize log buffer with specified size */
	warnx("log buffer size: %i bytes.", log_buffer_size);

	if (OK != logbuffer_init(&lb, log_buffer_size)) {
		errx(1, "can't allocate log buffer, exiting.");
	}

	struct vehicle_status_s buf_status;

	memset(&buf_status, 0, sizeof(buf_status));

	/* warning! using union here to save memory, elements should be used separately! */
	union {
		struct vehicle_command_s cmd;
		struct sensor_combined_s sensor;
		struct vehicle_attitude_s att;
		struct vehicle_attitude_setpoint_s att_sp;
		struct vehicle_rates_setpoint_s rates_sp;
		struct actuator_outputs_s act_outputs;
		struct actuator_controls_s act_controls;
		struct actuator_controls_effective_s act_controls_effective;
		struct vehicle_local_position_s local_pos;
		struct vehicle_local_position_setpoint_s local_pos_sp;
		struct vehicle_global_position_s global_pos;
		struct vehicle_global_position_setpoint_s global_pos_sp;
		struct vehicle_gps_position_s gps_pos;
		struct vehicle_vicon_position_s vicon_pos;
		struct optical_flow_s flow;
		struct rc_channels_s rc;
		struct differential_pressure_s diff_pres;
		struct airspeed_s airspeed;
		struct esc_status_s esc;
		struct vehicle_global_velocity_setpoint_s global_vel_sp;
	} buf;

	memset(&buf, 0, sizeof(buf));

	struct {
		int cmd_sub;
		int status_sub;
		int sensor_sub;
		int att_sub;
		int att_sp_sub;
		int rates_sp_sub;
		int act_outputs_sub;
		int act_controls_sub;
		int act_controls_effective_sub;
		int local_pos_sub;
		int local_pos_sp_sub;
		int global_pos_sub;
		int global_pos_sp_sub;
		int gps_pos_sub;
		int vicon_pos_sub;
		int flow_sub;
		int rc_sub;
		int airspeed_sub;
		int esc_sub;
		int global_vel_sp_sub;
	} subs;

	/* log message buffer: header + body */
#pragma pack(push, 1)
	struct {
		LOG_PACKET_HEADER;
		union {
			struct log_TIME_s log_TIME;
			struct log_ATT_s log_ATT;
			struct log_ATSP_s log_ATSP;
			struct log_IMU_s log_IMU;
			struct log_SENS_s log_SENS;
			struct log_LPOS_s log_LPOS;
			struct log_LPSP_s log_LPSP;
			struct log_GPS_s log_GPS;
			struct log_ATTC_s log_ATTC;
			struct log_STAT_s log_STAT;
			struct log_RC_s log_RC;
			struct log_OUT0_s log_OUT0;
			struct log_AIRS_s log_AIRS;
			struct log_ARSP_s log_ARSP;
			struct log_FLOW_s log_FLOW;
			struct log_GPOS_s log_GPOS;
			struct log_GPSP_s log_GPSP;
			struct log_ESC_s log_ESC;
			struct log_GVSP_s log_GVSP;
		} body;
	} log_msg = {
		LOG_PACKET_HEADER_INIT(0)
	};
#pragma pack(pop)
	memset(&log_msg.body, 0, sizeof(log_msg.body));

	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
	/* number of messages */
	const ssize_t fdsc = 20;
	/* Sanity check variable and index */
	ssize_t fdsc_count = 0;
	/* file descriptors to wait for */
	struct pollfd fds[fdsc];

	/* --- VEHICLE COMMAND --- */
	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
	fds[fdsc_count].fd = subs.cmd_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- VEHICLE STATUS --- */
	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
	fds[fdsc_count].fd = subs.status_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- GPS POSITION --- */
	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	fds[fdsc_count].fd = subs.gps_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- SENSORS COMBINED --- */
	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
	fds[fdsc_count].fd = subs.sensor_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ATTITUDE --- */
	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	fds[fdsc_count].fd = subs.att_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ATTITUDE SETPOINT --- */
	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	fds[fdsc_count].fd = subs.att_sp_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- RATES SETPOINT --- */
	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	fds[fdsc_count].fd = subs.rates_sp_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ACTUATOR OUTPUTS --- */
	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
	fds[fdsc_count].fd = subs.act_outputs_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ACTUATOR CONTROL --- */
	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	fds[fdsc_count].fd = subs.act_controls_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ACTUATOR CONTROL EFFECTIVE --- */
	subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
	fds[fdsc_count].fd = subs.act_controls_effective_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- LOCAL POSITION --- */
	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	fds[fdsc_count].fd = subs.local_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- LOCAL POSITION SETPOINT --- */
	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	fds[fdsc_count].fd = subs.local_pos_sp_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- GLOBAL POSITION --- */
	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	fds[fdsc_count].fd = subs.global_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- GLOBAL POSITION SETPOINT--- */
	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
	fds[fdsc_count].fd = subs.global_pos_sp_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- VICON POSITION --- */
	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
	fds[fdsc_count].fd = subs.vicon_pos_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- OPTICAL FLOW --- */
	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
	fds[fdsc_count].fd = subs.flow_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- RC CHANNELS --- */
	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
	fds[fdsc_count].fd = subs.rc_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- AIRSPEED --- */
	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	fds[fdsc_count].fd = subs.airspeed_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- ESCs --- */
	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
	fds[fdsc_count].fd = subs.esc_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* --- GLOBAL VELOCITY SETPOINT --- */
	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
	fds[fdsc_count].fd = subs.global_vel_sp_sub;
	fds[fdsc_count].events = POLLIN;
	fdsc_count++;

	/* WARNING: If you get the error message below,
	 * then the number of registered messages (fdsc)
	 * differs from the number of messages in the above list.
	 */
	if (fdsc_count > fdsc) {
		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__);
		fdsc_count = fdsc;
	}

	/*
	 * set up poll to block for new data,
	 * wait for a maximum of 1000 ms
	 */
	const int poll_timeout = 1000;

	thread_running = true;

	/* initialize thread synchronization */
	pthread_mutex_init(&logbuffer_mutex, NULL);
	pthread_cond_init(&logbuffer_cond, NULL);

	/* track changes in sensor_combined topic */
	uint16_t gyro_counter = 0;
	uint16_t accelerometer_counter = 0;
	uint16_t magnetometer_counter = 0;
	uint16_t baro_counter = 0;
	uint16_t differential_pressure_counter = 0;

	/* enable logging on start if needed */
	if (log_on_start)
		sdlog2_start_log();

	while (!main_thread_should_exit) {
		/* decide use usleep() or blocking poll() */
		bool use_sleep = sleep_delay > 0 && logging_enabled;

		/* poll all topics if logging enabled or only management (first 2) if not */
		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout);

		/* handle the poll result */
		if (poll_ret < 0) {
			warnx("ERROR: poll error, stop logging.");
			main_thread_should_exit = true;

		} else if (poll_ret > 0) {

			/* check all data subscriptions only if logging enabled,
			 * logging_enabled can be changed while checking vehicle_command and vehicle_status */
			bool check_data = logging_enabled;
			int ifds = 0;
			int handled_topics = 0;

			/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
				handle_command(&buf.cmd);
				handled_topics++;
			}

			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);

				if (log_when_armed) {
					handle_status(&buf_status);
				}

				handled_topics++;
			}

			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
				continue;
			}

			ifds = 1;	// begin from fds[1] again

			pthread_mutex_lock(&logbuffer_mutex);

			/* write time stamp message */
			log_msg.msg_type = LOG_TIME_MSG;
			log_msg.body.log_TIME.t = hrt_absolute_time();
			LOGBUFFER_WRITE_AND_COUNT(TIME);

			/* --- VEHICLE STATUS --- */
			if (fds[ifds++].revents & POLLIN) {
				// Don't orb_copy, it's already done few lines above
				log_msg.msg_type = LOG_STAT_MSG;
				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
				log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
				LOGBUFFER_WRITE_AND_COUNT(STAT);
			}

			/* --- GPS POSITION --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
				log_msg.msg_type = LOG_GPS_MSG;
				log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
				log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
				log_msg.body.log_GPS.eph = buf.gps_pos.eph_m;
				log_msg.body.log_GPS.epv = buf.gps_pos.epv_m;
				log_msg.body.log_GPS.lat = buf.gps_pos.lat;
				log_msg.body.log_GPS.lon = buf.gps_pos.lon;
				log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f;
				log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s;
				log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s;
				log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s;
				log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad;
				LOGBUFFER_WRITE_AND_COUNT(GPS);
			}

			/* --- SENSOR COMBINED --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
				bool write_IMU = false;
				bool write_SENS = false;

				if (buf.sensor.gyro_counter != gyro_counter) {
					gyro_counter = buf.sensor.gyro_counter;
					write_IMU = true;
				}

				if (buf.sensor.accelerometer_counter != accelerometer_counter) {
					accelerometer_counter = buf.sensor.accelerometer_counter;
					write_IMU = true;
				}

				if (buf.sensor.magnetometer_counter != magnetometer_counter) {
					magnetometer_counter = buf.sensor.magnetometer_counter;
					write_IMU = true;
				}

				if (buf.sensor.baro_counter != baro_counter) {
					baro_counter = buf.sensor.baro_counter;
					write_SENS = true;
				}

				if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
					differential_pressure_counter = buf.sensor.differential_pressure_counter;
					write_SENS = true;
				}

				if (write_IMU) {
					log_msg.msg_type = LOG_IMU_MSG;
					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
					LOGBUFFER_WRITE_AND_COUNT(IMU);
				}

				if (write_SENS) {
					log_msg.msg_type = LOG_SENS_MSG;
					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
					log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
					LOGBUFFER_WRITE_AND_COUNT(SENS);
				}
			}

			/* --- ATTITUDE --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
				log_msg.msg_type = LOG_ATT_MSG;
				log_msg.body.log_ATT.roll = buf.att.roll;
				log_msg.body.log_ATT.pitch = buf.att.pitch;
				log_msg.body.log_ATT.yaw = buf.att.yaw;
				log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
				log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
				log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
				LOGBUFFER_WRITE_AND_COUNT(ATT);
			}

			/* --- ATTITUDE SETPOINT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp);
				log_msg.msg_type = LOG_ATSP_MSG;
				log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
				log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
				log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
				log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
				LOGBUFFER_WRITE_AND_COUNT(ATSP);
			}

			/* --- RATES SETPOINT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp);
				log_msg.msg_type = LOG_ARSP_MSG;
				log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
				log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
				log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
				LOGBUFFER_WRITE_AND_COUNT(ARSP);
			}

			/* --- ACTUATOR OUTPUTS --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs);
				log_msg.msg_type = LOG_OUT0_MSG;
				memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
				LOGBUFFER_WRITE_AND_COUNT(OUT0);
			}

			/* --- ACTUATOR CONTROL --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls);
				log_msg.msg_type = LOG_ATTC_MSG;
				log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
				log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
				log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
				log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
				LOGBUFFER_WRITE_AND_COUNT(ATTC);
			}

			/* --- ACTUATOR CONTROL EFFECTIVE --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
				// TODO not implemented yet
			}

			/* --- LOCAL POSITION --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
				log_msg.msg_type = LOG_LPOS_MSG;
				log_msg.body.log_LPOS.x = buf.local_pos.x;
				log_msg.body.log_LPOS.y = buf.local_pos.y;
				log_msg.body.log_LPOS.z = buf.local_pos.z;
				log_msg.body.log_LPOS.vx = buf.local_pos.vx;
				log_msg.body.log_LPOS.vy = buf.local_pos.vy;
				log_msg.body.log_LPOS.vz = buf.local_pos.vz;
				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
				LOGBUFFER_WRITE_AND_COUNT(LPOS);
			}

			/* --- LOCAL POSITION SETPOINT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp);
				log_msg.msg_type = LOG_LPSP_MSG;
				log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
				log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
				log_msg.body.log_LPSP.z = buf.local_pos_sp.z;
				log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw;
				LOGBUFFER_WRITE_AND_COUNT(LPSP);
			}

			/* --- GLOBAL POSITION --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
				log_msg.msg_type = LOG_GPOS_MSG;
				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
				LOGBUFFER_WRITE_AND_COUNT(GPOS);
			}

			/* --- GLOBAL POSITION SETPOINT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
				log_msg.msg_type = LOG_GPSP_MSG;
				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
				LOGBUFFER_WRITE_AND_COUNT(GPSP);
			}

			/* --- VICON POSITION --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
				// TODO not implemented yet
			}

			/* --- FLOW --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
				log_msg.msg_type = LOG_FLOW_MSG;
				log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
				log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
				log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
				log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
				log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
				log_msg.body.log_FLOW.quality = buf.flow.quality;
				log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
				LOGBUFFER_WRITE_AND_COUNT(FLOW);
			}

			/* --- RC CHANNELS --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc);
				log_msg.msg_type = LOG_RC_MSG;
				/* Copy only the first 8 channels of 14 */
				memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
				LOGBUFFER_WRITE_AND_COUNT(RC);
			}

			/* --- AIRSPEED --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
				log_msg.msg_type = LOG_AIRS_MSG;
				log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
				log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
				LOGBUFFER_WRITE_AND_COUNT(AIRS);
			}

			/* --- ESCs --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);

				for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
					log_msg.msg_type = LOG_ESC_MSG;
					log_msg.body.log_ESC.counter = buf.esc.counter;
					log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
					log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
					log_msg.body.log_ESC.esc_num = i;
					log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
					log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
					log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
					log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
					log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
					log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
					log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
					log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
					LOGBUFFER_WRITE_AND_COUNT(ESC);
				}
			}

			/* --- GLOBAL VELOCITY SETPOINT --- */
			if (fds[ifds++].revents & POLLIN) {
				orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
				log_msg.msg_type = LOG_GVSP_MSG;
				log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
				log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
				log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
				LOGBUFFER_WRITE_AND_COUNT(GVSP);
			}

			/* signal the other thread new data, but not yet unlock */
			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
				/* only request write if several packets can be written at once */
				pthread_cond_signal(&logbuffer_cond);
			}

			/* unlock, now the writer thread may run */
			pthread_mutex_unlock(&logbuffer_mutex);
		}

		if (use_sleep) {
			usleep(sleep_delay);
		}
	}

	if (logging_enabled)
		sdlog2_stop_log();

	pthread_mutex_destroy(&logbuffer_mutex);
	pthread_cond_destroy(&logbuffer_cond);

	free(lb.data);

	warnx("exiting.");

	thread_running = false;

	return 0;
}
示例#26
0
void
Navigator::navigation_capabilities_update()
{
	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
示例#27
0
void
FixedwingPositionControl::task_main()
{

	/* inform about start */
	warnx("Initializing..");
	fflush(stdout);

	/*
	 * do subscriptions
	 */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_control_mode_sub, 200);
	/* rate limit position updates to 50 Hz */
	orb_set_interval(_global_pos_sub, 20);

	/* abort on a nonzero return value from the parameter init */
	if (parameters_update()) {
		/* parameter setup went wrong, abort */
		warnx("aborting startup due to errors.");
		_task_should_exit = true;
	}

	/* wakeup source(s) */
	struct pollfd fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _global_pos_sub;
	fds[1].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* check vehicle status for changes to publication state */
		vehicle_control_mode_poll();

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run controller if position changed */
		if (fds[1].revents & POLLIN) {

			/* XXX Hack to get mavlink output going */
			if (_mavlink_fd < 0) {
				/* try to open the mavlink log device every once in a while */
				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
			}

			static uint64_t last_run = 0;
			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too large deltaT's */
			if (deltaT > 1.0f)
				deltaT = 0.01f;

			/* load local copies */
			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);

			// XXX add timestamp check
			_global_pos_valid = true;

			vehicle_attitude_poll();
			vehicle_setpoint_poll();
			vehicle_sensor_combined_poll();
			vehicle_airspeed_poll();
			range_finder_poll();
			// vehicle_baro_poll();

			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);

			/*
			 * Attempt to control position, on success (= sensors present and not in manual mode),
			 * publish setpoint.
			 */
			if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
				_att_sp.timestamp = hrt_absolute_time();

				/* lazily publish the setpoint only once available */
				if (_attitude_sp_pub > 0) {
					/* publish the attitude setpoint */
					orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);

				} else {
					/* advertise and publish */
					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
				}

				/* XXX check if radius makes sense here */
				float turn_distance = _l1_control.switch_distance(100.0f);

				/* lazily publish navigation capabilities */
				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {

					/* set new turn distance */
					_nav_capabilities.turn_distance = turn_distance;

					navigation_capabilities_publish();

				}

			}

		}

		perf_end(_loop_perf);
	}

	warnx("exiting.\n");

	_control_task = -1;
	_exit(0);
}
示例#28
0
void
Navigator::vehicle_land_detected_update()
{
	orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
}
示例#29
0
void
Navigator::sensor_combined_update()
{
	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
示例#30
0
int do_airspeed_calibration(int mavlink_fd)
{
    /* give directions */
    mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
    mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");

    const int calibration_count = 2000;

    int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
    struct differential_pressure_s diff_pres;

    int calibration_counter = 0;
    float diff_pres_offset = 0.0f;

    /* Reset sensor parameters */
    struct airspeed_scale airscale = {
        0.0f,
        1.0f,
    };

    bool paramreset_successful = false;
    int  fd = open(AIRSPEED_DEVICE_PATH, 0);
    if (fd > 0) {
        if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
            paramreset_successful = true;
        } else {
            mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
        }
        close(fd);
    }

    if (!paramreset_successful) {
        warn("FAILED to set scale / offsets for airspeed");
        mavlink_log_critical(mavlink_fd, "dpress reset failed");
        mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
        return ERROR;
    }

    while (calibration_counter < calibration_count) {

        /* wait blocking for new data */
        struct pollfd fds[1];
        fds[0].fd = diff_pres_sub;
        fds[0].events = POLLIN;

        int poll_ret = poll(fds, 1, 1000);

        if (poll_ret) {
            orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
            diff_pres_offset += diff_pres.differential_pressure_raw_pa;
            calibration_counter++;

            if (calibration_counter % (calibration_count / 20) == 0)
                mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);

        } else if (poll_ret == 0) {
            /* any poll failure for 1s is a reason to abort */
            mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
            close(diff_pres_sub);
            return ERROR;
        }
    }

    diff_pres_offset = diff_pres_offset / calibration_count;

    if (isfinite(diff_pres_offset)) {

        if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
            mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
            close(diff_pres_sub);
            return ERROR;
        }

        /* auto-save to EEPROM */
        int save_ret = param_save_default();

        if (save_ret != 0) {
            warn("WARNING: auto-save of params to storage failed");
            mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
            close(diff_pres_sub);
            return ERROR;
        }

        mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
        tune_neutral(true);
        close(diff_pres_sub);
        return OK;

    } else {
        mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
        close(diff_pres_sub);
        return ERROR;
    }
}