void estimator_base::gps_poll()
{
    _gps_new = false;

    orb_check(_gps_sub, &_gps_new);

    if (_gps_new) {
        orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
    }

    if (_gps.fix_type < 3 || !isfinite(_gps.lon) || !isfinite(_gps.lat) || !isfinite(_gps.alt) ) {
        _gps_new = false;
    }

    if (_gps_new && !_gps_init) {
        _gps_init = true;
        _init_lon = _gps.lon;
        _init_lat = _gps.lat;
        _init_alt = _gps.alt;
    }
}
void
MulticopterAttitudeControl::vehicle_status_poll()
{
	/* check if there is new status information */
	bool vehicle_status_updated;
	orb_check(_vehicle_status_sub, &vehicle_status_updated);

	if (vehicle_status_updated) {
		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
		/* set correct uORB ID, depending on if vehicle is VTOL or not */
		if (!_rates_sp_id) {
			if (_vehicle_status.is_vtol) {
				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
			} else {
				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
				_actuators_id = ORB_ID(actuator_controls_0);
			}
		}
	}
}
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
	bool vstatus_updated;

	/* Check HIL state if vehicle status has changed */
	orb_check(_control_mode_sub, &vstatus_updated);

	if (vstatus_updated) {

		bool was_armed = _control_mode.flag_armed;

		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);

		if (!was_armed && _control_mode.flag_armed) {
			_launch_lat = _global_pos.lat;
			_launch_lon = _global_pos.lon;
			_launch_alt = _global_pos.alt;
			_launch_valid = true;
		}
	}
}
void MulticopterLandDetector::updateParameterCache(const bool force)
{
	bool updated;
	parameter_update_s paramUpdate;

	orb_check(_parameterSub, &updated);

	if (updated) {
		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
	}

	if (updated || force) {
		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
		param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
	}
}
Exemple #5
0
void OutputBase::_handle_position_update(bool force_update)
{
	bool need_update = force_update;

	if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
		return;
	}

	if (!force_update) {
		orb_check(_vehicle_global_position_sub, &need_update);
	}

	if (!need_update) {
		return;
	}

	vehicle_global_position_s vehicle_global_position;
	orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);

	float pitch;
	const double &lon = _cur_control_data->type_data.lonlat.lon;
	const double &lat = _cur_control_data->type_data.lonlat.lat;
	const float &alt = _cur_control_data->type_data.lonlat.altitude;

	if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
		pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle;

	} else {
		pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position);
	}

	float roll = _cur_control_data->type_data.lonlat.roll_angle;
	float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon)
		    - vehicle_global_position.yaw;

	_angle_setpoints[0] = roll;
	_angle_setpoints[1] = pitch + _cur_control_data->type_data.lonlat.pitch_angle_offset;
	_angle_setpoints[2] = yaw + _cur_control_data->type_data.lonlat.yaw_angle_offset;
}
bool
MavlinkOrbSubscription::is_published()
{
	// If we marked it as published no need to check again
	if (_published) {
		return true;
	}

	hrt_abstime now = hrt_absolute_time();

	if (now - _last_pub_check < 300000) {
		return false;
	}

	// We are checking now
	_last_pub_check = now;

	// We don't want to subscribe to anything that does not exist
	// in order to save memory and file descriptors.
	// However, for some topics like vehicle_command_ack, we want to subscribe
	// from the beginning in order not to miss or delay the first publish respective advertise.
	if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
		return false;
	}

	if (_fd < 0) {
		_fd = orb_subscribe_multi(_topic, _instance);
	}

	bool updated;
	orb_check(_fd, &updated);

	if (updated) {
		_published = true;
	}

	return _published;
}
bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
	bool prevpub = _published;

	if (!is_published()) {
		return false;
	}

	bool updated;

	if (orb_check(_fd, &updated)) {
		return false;
	}

	// If we didn't update and this topic did not change
	// its publication status then nothing really changed
	if (!updated && prevpub == _published) {
		return false;
	}

	return update(data);
}
Exemple #8
0
void GPS::handleInjectDataTopic()
{
    if (_orb_inject_data_fd[0] == -1) {
        return;
    }

    bool updated = false;

    do {
        int orb_inject_data_cur_fd = _orb_inject_data_fd[_orb_inject_data_next];
        orb_check(orb_inject_data_cur_fd, &updated);

        if (updated) {
            struct gps_inject_data_s msg;
            orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
            injectData(msg.data, msg.len);

            _orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;

            ++_last_rate_rtcm_injection_count;
        }
    } while (updated);
}
int do_trim_calibration(int mavlink_fd)
{
	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
	usleep(200000);
	struct manual_control_setpoint_s sp;
	bool changed;
	orb_check(sub_man, &changed);

	if (!changed) {
		mavlink_log_critical(mavlink_fd, "no inputs, aborting");
		return ERROR;
	}

	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);

	/* set parameters */
	float p = sp.roll;
	param_set(param_find("TRIM_ROLL"), &p);
	p = sp.pitch;
	param_set(param_find("TRIM_PITCH"), &p);
	p = sp.yaw;
	param_set(param_find("TRIM_YAW"), &p);

	/* store to permanent storage */
	/* auto-save */
	int save_ret = param_save_default();

	if (save_ret != 0) {
		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
		close(sub_man);
		return ERROR;
	}

	mavlink_log_info(mavlink_fd, "trim cal done");
	close(sub_man);
	return OK;
}
void
MulticopterPositionControl::poll_subscriptions()
{
	bool updated;

	orb_check(_att_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
	}

	orb_check(_att_sp_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
	}

	orb_check(_control_mode_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
	}

	orb_check(_manual_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
	}

	orb_check(_arming_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
	}

	orb_check(_local_pos_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
	}

}
Exemple #11
0
void Ekf2::task_main()
{
	// subscribe to relevant topics
	int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	int params_sub = orb_subscribe(ORB_ID(parameter_update));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
	int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
	int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	int status_sub = orb_subscribe(ORB_ID(vehicle_status));

	px4_pollfd_struct_t fds[2] = {};
	fds[0].fd = sensors_sub;
	fds[0].events = POLLIN;
	fds[1].fd = params_sub;
	fds[1].events = POLLIN;

	// initialise parameter cache
	updateParams();

	// initialize data structures outside of loop
	// because they will else not always be
	// properly populated
	sensor_combined_s sensors = {};
	vehicle_gps_position_s gps = {};
	airspeed_s airspeed = {};
	optical_flow_s optical_flow = {};
	distance_sensor_s range_finder = {};
	vehicle_land_detected_s vehicle_land_detected = {};
	vehicle_local_position_s ev_pos = {};
	vehicle_attitude_s ev_att = {};
	vehicle_status_s vehicle_status = {};

	while (!_task_should_exit) {
		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout or no new data, do nothing
			continue;
		}

		if (fds[1].revents & POLLIN) {
			// read from param to clear updated flag
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), params_sub, &update);
			updateParams();

			// fetch sensor data in next loop
			continue;

		} else if (!(fds[0].revents & POLLIN)) {
			// no new data
			continue;
		}

		bool gps_updated = false;
		bool airspeed_updated = false;
		bool optical_flow_updated = false;
		bool range_finder_updated = false;
		bool vehicle_land_detected_updated = false;
		bool vision_position_updated = false;
		bool vision_attitude_updated = false;
		bool vehicle_status_updated = false;

		orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
		// update all other topics if they have new data

		orb_check(status_sub, &vehicle_status_updated);

		if (vehicle_status_updated) {
			orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
		}

		orb_check(gps_sub, &gps_updated);

		if (gps_updated) {
			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
		}

		orb_check(airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
		}

		orb_check(optical_flow_sub, &optical_flow_updated);

		if (optical_flow_updated) {
			orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
		}

		orb_check(range_finder_sub, &range_finder_updated);

		if (range_finder_updated) {
			orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder);

			if (range_finder.min_distance > range_finder.current_distance
			    || range_finder.max_distance < range_finder.current_distance) {
				range_finder_updated = false;
			}
		}

		orb_check(ev_pos_sub, &vision_position_updated);

		if (vision_position_updated) {
			orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
		}

		orb_check(ev_att_sub, &vision_attitude_updated);

		if (vision_attitude_updated) {
			orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
		}

		// in replay mode we are getting the actual timestamp from the sensor topic
		hrt_abstime now = 0;

		if (_replay_mode) {
			now = sensors.timestamp;

		} else {
			now = hrt_absolute_time();
		}

		// push imu data into estimator
		float gyro_integral[3];
		gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
		gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
		gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
		float accel_integral[3];
		accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
		accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
		accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
		_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
				gyro_integral, accel_integral);

		// read mag data
		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			// set a zero timestamp to let the ekf replay program know that this data is not valid
			_timestamp_mag_us = 0;

		} else {
			if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) {
				_timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative;

				// If the time last used by the EKF is less than specified, then accumulate the
				// data and push the average when the 50msec is reached.
				_mag_time_sum_ms += _timestamp_mag_us / 1000;
				_mag_sample_count++;
				_mag_data_sum[0] += sensors.magnetometer_ga[0];
				_mag_data_sum[1] += sensors.magnetometer_ga[1];
				_mag_data_sum[2] += sensors.magnetometer_ga[2];
				uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count;

				if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) {
					float mag_sample_count_inv = 1.0f / (float)_mag_sample_count;
					float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv, _mag_data_sum[1] *mag_sample_count_inv, _mag_data_sum[2] *mag_sample_count_inv};
					_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);
					_mag_time_ms_last_used = mag_time_ms;
					_mag_time_sum_ms = 0;
					_mag_sample_count = 0;
					_mag_data_sum[0] = 0.0f;
					_mag_data_sum[1] = 0.0f;
					_mag_data_sum[2] = 0.0f;

				}
			}
		}

		// read baro data
		if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			// set a zero timestamp to let the ekf replay program know that this data is not valid
			_timestamp_balt_us = 0;

		} else {
			if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) {
				_timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative;

				// If the time last used by the EKF is less than specified, then accumulate the
				// data and push the average when the 50msec is reached.
				_balt_time_sum_ms += _timestamp_balt_us / 1000;
				_balt_sample_count++;
				_balt_data_sum += sensors.baro_alt_meter;
				uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;

				if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) {
					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
					_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
					_balt_time_ms_last_used = balt_time_ms;
					_balt_time_sum_ms = 0;
					_balt_sample_count = 0;
					_balt_data_sum = 0.0f;

				}
			}
		}

		// read gps data if available
		if (gps_updated) {
			struct gps_message gps_msg = {};
			gps_msg.time_usec = gps.timestamp;
			gps_msg.lat = gps.lat;
			gps_msg.lon = gps.lon;
			gps_msg.alt = gps.alt;
			gps_msg.fix_type = gps.fix_type;
			gps_msg.eph = gps.eph;
			gps_msg.epv = gps.epv;
			gps_msg.sacc = gps.s_variance_m_s;
			gps_msg.vel_m_s = gps.vel_m_s;
			gps_msg.vel_ned[0] = gps.vel_n_m_s;
			gps_msg.vel_ned[1] = gps.vel_e_m_s;
			gps_msg.vel_ned[2] = gps.vel_d_m_s;
			gps_msg.vel_ned_valid = gps.vel_ned_valid;
			gps_msg.nsats = gps.satellites_used;
			//TODO add gdop to gps topic
			gps_msg.gdop = 0.0f;

			_ekf.setGpsData(gps.timestamp, &gps_msg);

		}

		// only set airspeed data if condition for airspeed fusion are met
		bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing
				     && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f;

		if (fuse_airspeed) {
			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
			_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
		}

		// only fuse synthetic sideslip measurements if conditions are met
		bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get();
		_ekf.set_fuse_beta_flag(fuse_beta);

		if (optical_flow_updated) {
			flow_message flow;
			flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
			flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
			flow.quality = optical_flow.quality;
			flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
			flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
			flow.gyrodata(2) = optical_flow.gyro_z_rate_integral;
			flow.dt = optical_flow.integration_timespan;

			if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
			    PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
				_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
			}
		}

		if (range_finder_updated) {
			_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
		}

		// get external vision data
		// if error estimates are unavailable, use parameter defined defaults
		if (vision_position_updated || vision_attitude_updated) {
			ext_vision_message ev_data;
			ev_data.posNED(0) = ev_pos.x;
			ev_data.posNED(1) = ev_pos.y;
			ev_data.posNED(2) = ev_pos.z;
			Quaternion q(ev_att.q);
			ev_data.quat = q;

			// position measurement error from parameters. TODO : use covariances from topic
			ev_data.posErr = _default_ev_pos_noise;
			ev_data.angErr = _default_ev_ang_noise;

			// use timestamp from external computer, clocks are synchronized when using MAVROS
			_ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
		}

		orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);

		if (vehicle_land_detected_updated) {
			orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected);
			_ekf.set_in_air_status(!vehicle_land_detected.landed);
		}

		// run the EKF update and output
		if (_ekf.update()) {

			matrix::Quaternion<float> q;
			_ekf.copy_quaternion(q.data());

			float velocity[3];
			_ekf.get_velocity(velocity);

			float gyro_rad[3];

			{
				// generate control state data
				control_state_s ctrl_state = {};
				float gyro_bias[3] = {};
				_ekf.get_gyro_bias(gyro_bias);
				ctrl_state.timestamp = _replay_mode ? now : hrt_absolute_time();
				gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
				gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
				gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
				ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
				ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
				ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
				ctrl_state.roll_rate_bias = gyro_bias[0];
				ctrl_state.pitch_rate_bias = gyro_bias[1];
				ctrl_state.yaw_rate_bias = gyro_bias[2];

				// Velocity in body frame
				Vector3f v_n(velocity);
				matrix::Dcm<float> R_to_body(q.inversed());
				Vector3f v_b = R_to_body * v_n;
				ctrl_state.x_vel = v_b(0);
				ctrl_state.y_vel = v_b(1);
				ctrl_state.z_vel = v_b(2);


				// Local Position NED
				float position[3];
				_ekf.get_position(position);
				ctrl_state.x_pos = position[0];
				ctrl_state.y_pos = position[1];
				ctrl_state.z_pos = position[2];

				// Attitude quaternion
				q.copyTo(ctrl_state.q);

				_ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter);

				// Acceleration data
				matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);

				float accel_bias[3];
				_ekf.get_accel_bias(accel_bias);
				ctrl_state.x_acc = acceleration(0) - accel_bias[0];
				ctrl_state.y_acc = acceleration(1) - accel_bias[1];
				ctrl_state.z_acc = acceleration(2) - accel_bias[2];

				// compute lowpass filtered horizontal acceleration
				acceleration = R_to_body.transpose() * acceleration;
				_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) +
						acceleration(1) * acceleration(1));
				ctrl_state.horz_acc_mag = _acc_hor_filt;

				ctrl_state.airspeed_valid = false;

				// use estimated velocity for airspeed estimate
				if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
					// use measured airspeed
					if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
					    && airspeed.timestamp > 0) {
						ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
						ctrl_state.airspeed_valid = true;
					}

				} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
					if (_ekf.local_position_is_valid()) {
						ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
						ctrl_state.airspeed_valid = true;
					}

				} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
					// do nothing, airspeed has been declared as non-valid above, controllers
					// will handle this assuming always trim airspeed
				}

				// publish control state data
				if (_control_state_pub == nullptr) {
					_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);

				} else {
					orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
				}
			}


			{
				// generate vehicle attitude quaternion data
				struct vehicle_attitude_s att = {};
				att.timestamp = _replay_mode ? now : hrt_absolute_time();

				q.copyTo(att.q);

				att.rollspeed = gyro_rad[0];
				att.pitchspeed = gyro_rad[1];
				att.yawspeed = gyro_rad[2];

				// publish vehicle attitude data
				if (_att_pub == nullptr) {
					_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

				} else {
					orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
				}
			}

			// generate vehicle local position data
			struct vehicle_local_position_s lpos = {};
			float pos[3] = {};

			lpos.timestamp = _replay_mode ? now : hrt_absolute_time();

			// Position of body origin in local NED frame
			_ekf.get_position(pos);
			lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f;
			lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f;
			lpos.z = pos[2];

			// Velocity of body origin in local NED frame (m/s)
			lpos.vx = velocity[0];
			lpos.vy = velocity[1];
			lpos.vz = velocity[2];

			// TODO: better status reporting
			lpos.xy_valid = _ekf.local_position_is_valid();
			lpos.z_valid = true;
			lpos.v_xy_valid = _ekf.local_position_is_valid();
			lpos.v_z_valid = true;

			// Position of local NED origin in GPS / WGS84 frame
			struct map_projection_reference_s ekf_origin = {};
			// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
			_ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
			lpos.xy_global = _ekf.global_position_is_valid();
			lpos.z_global = true;                                // true if z is valid and has valid global reference (ref_alt)
			lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
			lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees

			// The rotation of the tangent plane vs. geographical north
			matrix::Eulerf euler(q);
			lpos.yaw = euler.psi();

			float terrain_vpos;
			lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);
			lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
			lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate
			lpos.surface_bottom_timestamp	= hrt_absolute_time(); // Time when new bottom surface found

			// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
			Vector3f pos_var, vel_var;
			_ekf.get_pos_var(pos_var);
			_ekf.get_vel_var(vel_var);
			lpos.eph = sqrtf(pos_var(0) + pos_var(1));
			lpos.epv = sqrtf(pos_var(2));

			// get state reset information of position and velocity
			_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
			_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
			_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
			_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);

			// publish vehicle local position data
			if (_lpos_pub == nullptr) {
				_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);

			} else {
				orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
			}

			if (_ekf.global_position_is_valid()) {
				// generate and publish global position data
				struct vehicle_global_position_s global_pos = {};

				global_pos.timestamp = _replay_mode ? now : hrt_absolute_time();
				global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds

				double est_lat, est_lon, lat_pre_reset, lon_pre_reset;
				map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
				global_pos.lat = est_lat; // Latitude in degrees
				global_pos.lon = est_lon; // Longitude in degrees
				map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset,
							 &lon_pre_reset);
				global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset;
				global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset;
				global_pos.lat_lon_reset_counter = lpos.xy_reset_counter;

				global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
				_ekf.get_posD_reset(&global_pos.delta_alt, &global_pos.alt_reset_counter);
				// global altitude has opposite sign of local down position
				global_pos.delta_alt *= -1.0f;

				global_pos.vel_n = velocity[0]; // Ground north velocity, m/s
				global_pos.vel_e = velocity[1]; // Ground east velocity, m/s
				global_pos.vel_d = velocity[2]; // Ground downside velocity, m/s

				global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI.

				global_pos.eph = sqrtf(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
				global_pos.epv = sqrtf(pos_var(2)); // Standard deviation of position vertically

				if (lpos.dist_bottom_valid) {
					global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
					global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid

				} else {
					global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
					global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
				}

				// TODO use innovatun consistency check timouts to set this
				global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning

				global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)

				if (_vehicle_global_position_pub == nullptr) {
					_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);
				}
			}

		} else if (_replay_mode) {
			// in replay mode we have to tell the replay module not to wait for an update
			// we do this by publishing an attitude with zero timestamp
			struct vehicle_attitude_s att = {};
			att.timestamp = now;

			if (_att_pub == nullptr) {
				_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

			} else {
				orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
			}
		}

		// publish estimator status
		struct estimator_status_s status = {};
		status.timestamp = _replay_mode ? now : hrt_absolute_time();
		_ekf.get_state_delayed(status.states);
		_ekf.get_covariances(status.covariances);
		_ekf.get_gps_check_status(&status.gps_check_fail_flags);
		_ekf.get_control_mode(&status.control_mode_flags);
		_ekf.get_filter_fault_status(&status.filter_fault_flags);
		_ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio,
						&status.vel_test_ratio, &status.pos_test_ratio,
						&status.hgt_test_ratio, &status.tas_test_ratio,
						&status.hagl_test_ratio);
		bool dead_reckoning;
		_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy, &dead_reckoning);
		_ekf.get_ekf_soln_status(&status.solution_status_flags);
		_ekf.get_imu_vibe_metrics(status.vibe);

		if (_estimator_status_pub == nullptr) {
			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);

		} else {
			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
		}

		// Publish wind estimate
		struct wind_estimate_s wind_estimate = {};
		wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time();
		wind_estimate.windspeed_north = status.states[22];
		wind_estimate.windspeed_east = status.states[23];
		wind_estimate.covariance_north = status.covariances[22];
		wind_estimate.covariance_east = status.covariances[23];

		if (_wind_pub == nullptr) {
			_wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate);

		} else {
			orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate);
		}

		// publish estimator innovation data
		{
			struct ekf2_innovations_s innovations = {};
			innovations.timestamp = _replay_mode ? now : hrt_absolute_time();
			_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
			_ekf.get_mag_innov(&innovations.mag_innov[0]);
			_ekf.get_heading_innov(&innovations.heading_innov);
			_ekf.get_airspeed_innov(&innovations.airspeed_innov);
			_ekf.get_beta_innov(&innovations.beta_innov);
			_ekf.get_flow_innov(&innovations.flow_innov[0]);
			_ekf.get_hagl_innov(&innovations.hagl_innov);

			_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
			_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
			_ekf.get_heading_innov_var(&innovations.heading_innov_var);
			_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
			_ekf.get_beta_innov_var(&innovations.beta_innov_var);
			_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
			_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);

			_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);

			if (_estimator_innovations_pub == nullptr) {
				_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);

			} else {
				orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
			}

		}

		// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
		if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
			float decl_deg;
			_ekf.copy_mag_decl_deg(&decl_deg);
			_mag_declination_deg.set(decl_deg);
		}

		_prev_landed = vehicle_land_detected.landed;

		// publish ekf2_timestamps (using 0.1 ms relative timestamps)
		{
			ekf2_timestamps_s ekf2_timestamps;
			ekf2_timestamps.timestamp = sensors.timestamp;

			if (gps_updated) {
				// divide individually to get consistent rounding behavior
				ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (optical_flow_updated) {
				ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (range_finder_updated) {
				ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (airspeed_updated) {
				ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (vision_position_updated) {
				ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (vision_attitude_updated) {
				ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (_ekf2_timestamps_pub == nullptr) {
				_ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps);

			} else {
				orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps);
			}
		}


		// publish replay message if in replay mode
		bool publish_replay_message = (bool)_param_record_replay_msg.get();

		if (publish_replay_message) {
			struct ekf2_replay_s replay = {};
			replay.timestamp = now;
			replay.gyro_integral_dt = sensors.gyro_integral_dt;
			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
			replay.magnetometer_timestamp = _timestamp_mag_us;
			replay.baro_timestamp = _timestamp_balt_us;
			memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
			memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
			memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
			replay.baro_alt_meter = sensors.baro_alt_meter;

			// only write gps data if we had a gps update.
			if (gps_updated) {
				replay.time_usec = gps.timestamp;
				replay.lat = gps.lat;
				replay.lon = gps.lon;
				replay.alt = gps.alt;
				replay.fix_type = gps.fix_type;
				replay.nsats = gps.satellites_used;
				replay.eph = gps.eph;
				replay.epv = gps.epv;
				replay.sacc = gps.s_variance_m_s;
				replay.vel_m_s = gps.vel_m_s;
				replay.vel_n_m_s = gps.vel_n_m_s;
				replay.vel_e_m_s = gps.vel_e_m_s;
				replay.vel_d_m_s = gps.vel_d_m_s;
				replay.vel_ned_valid = gps.vel_ned_valid;

			} else {
				// this will tell the logging app not to bother logging any gps replay data
				replay.time_usec = 0;
			}

			if (optical_flow_updated) {
				replay.flow_timestamp = optical_flow.timestamp;
				replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral;
				replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral;
				replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
				replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
				replay.flow_time_integral = optical_flow.integration_timespan;
				replay.flow_quality = optical_flow.quality;

			} else {
				replay.flow_timestamp = 0;
			}

			if (range_finder_updated) {
				replay.rng_timestamp = range_finder.timestamp;
				replay.range_to_ground = range_finder.current_distance;

			} else {
				replay.rng_timestamp = 0;
			}

			if (airspeed_updated) {
				replay.asp_timestamp = airspeed.timestamp;
				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;

			} else {
				replay.asp_timestamp = 0;
			}

			if (vision_position_updated || vision_attitude_updated) {
				replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp;
				replay.pos_ev[0] = ev_pos.x;
				replay.pos_ev[1] = ev_pos.y;
				replay.pos_ev[2] = ev_pos.z;
				replay.quat_ev[0] = ev_att.q[0];
				replay.quat_ev[1] = ev_att.q[1];
				replay.quat_ev[2] = ev_att.q[2];
				replay.quat_ev[3] = ev_att.q[3];
				// TODO : switch to covariances from topic later
				replay.pos_err = _default_ev_pos_noise;
				replay.ang_err = _default_ev_ang_noise;

			} else {
				replay.ev_timestamp = 0;
			}

			if (_replay_pub == nullptr) {
				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);

			} else {
				orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
			}
		}
	}

	orb_unsubscribe(sensors_sub);
	orb_unsubscribe(gps_sub);
	orb_unsubscribe(airspeed_sub);
	orb_unsubscribe(params_sub);
	orb_unsubscribe(optical_flow_sub);
	orb_unsubscribe(range_finder_sub);
	orb_unsubscribe(ev_pos_sub);
	orb_unsubscribe(vehicle_land_detected_sub);
	orb_unsubscribe(status_sub);

	delete ekf2::instance;
	ekf2::instance = nullptr;
}
/****************************************************************************
 * 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 != baro_timestamp) {
					baro_timestamp = sensor.baro_timestamp;

					/* mean calculation over several measurements */
					if (baro_init_cnt < baro_init_num) {
						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
							baro_offset += sensor.baro_alt_meter;
							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 != 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;
					accel_updates++;
				}

				if (sensor.baro_timestamp != baro_timestamp) {
					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
					baro_timestamp = sensor.baro_timestamp;
					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;
}
/*
 * 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[])
{

	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 };

	/* Initialize filter */
	AttitudeEKF_initialize();

	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));

	/* 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));

	/* subscribe to mocap data */
	int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));

	/* 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_s vision {};
	struct att_pos_mocap_s mocap {};

	/* register the perf counter */
	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");

	/* Main loop*/
	while (!thread_should_exit) {

		px4_pollfd_struct_t fds[2];
		fds[0].fd = sub_raw;
		fds[0].events = POLLIN;
		fds[1].fd = sub_params;
		fds[1].events = POLLIN;
		int ret = px4_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 sensor data - 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) < 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.gyro_timestamp[0]) {
						update_vect[0] = 1;
						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
					}

					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[0]) {
						update_vect[1] = 1;
						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
						sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
					}

					hrt_abstime vel_t = 0;
					bool vel_valid = false;
					if (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[0] &&
						/* check that the mag vector is > 0 */
						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
						update_vect[2] = 1;
						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
						sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
					}

					bool vision_updated = false;
					orb_check(vision_sub, &vision_updated);

					bool mocap_updated = false;
					orb_check(mocap_sub, &mocap_updated);

					if (vision_updated) {
						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
					}

					if (mocap_updated) {
						orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
					}

					if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) {

						math::Quaternion q(mocap.q);
						math::Matrix<3, 3> Rmoc = q.to_dcm();

						math::Vector<3> v(1.0f, 0.0f, 0.4f);

						math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw
											    // Rrw * Vw = vn. This way we have consistency
						z_k[6] = vn(0);
						z_k[7] = vn(1);
						z_k[8] = vn(2);
					}else 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.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
											    // Rrw * Vw = vn. This way we have consistency
						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];
					}

					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) < 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);

						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 (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_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) {
						warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(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 (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
						&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
						// Broadcast
						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

					} else {
						warnx("ERR: NaN estimate!");
					}

					perf_end(ekf_loop_perf);
				}
			}
		}

		loopcounter++;
	}

	thread_running = false;

	return 0;
}
Exemple #14
0
void
Delivery::to_destination()
{
	// set a mission to destination with takeoff enabled
	// Status = enroute ; change to Dropoff at completion

	if (_complete) {
		// Update status now that travel to destination is complete and reset _first_run for next stage
		_first_run = true;
		mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk at Location");
		advance_delivery();
		return;
	}

	check_mission_valid();

	/* check if anything has changed */
	bool onboard_updated = false;
	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
	if (onboard_updated) {
		update_onboard_mission();
	}

	bool offboard_updated = false;
	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
	if (offboard_updated) {
		update_offboard_mission();
	}

	/* reset mission items if needed */
	if (onboard_updated || offboard_updated) {
		set_mission_items();
	}

	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
		set_mission_item_reached();
		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
			mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk advancing mission");
		}

	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
		altitude_sp_foh_update();
	} else {
		/* if waypoint position reached allow loiter on the setpoint */
		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
			_navigator->set_can_loiter_at_sp(true);
			_complete = true;
		}
	}

	/* see if we need to update the current yaw heading for rotary wing types */
	if (_navigator->get_vstatus()->is_rotary_wing 
			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
			&& _mission_type != MISSION_TYPE_NONE) {
		heading_sp_update();
	}

}
Exemple #15
0
void
BlinkM::led()
{

	static int vehicle_status_sub_fd;
	static int vehicle_gps_position_sub_fd;

	static int num_of_cells = 0;
	static int detected_cells_runcount = 0;

	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
	static int t_led_blink = 0;
	static int led_thread_runcount=0;
	static int led_interval = 1000;

	static int no_data_vehicle_status = 0;
	static int no_data_vehicle_gps_position = 0;

	static bool topic_initialized = false;
	static bool detected_cells_blinked = false;
	static bool led_thread_ready = true;

	int num_of_used_sats = 0;

	if(!topic_initialized) {
		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
		orb_set_interval(vehicle_status_sub_fd, 1000);

		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
		orb_set_interval(vehicle_gps_position_sub_fd, 1000);

		topic_initialized = true;
	}

	if(led_thread_ready == true) {
		if(!detected_cells_blinked) {
			if(num_of_cells > 0) {
				t_led_color[0] = LED_PURPLE;
			}
			if(num_of_cells > 1) {
				t_led_color[1] = LED_PURPLE;
			}
			if(num_of_cells > 2) {
				t_led_color[2] = LED_PURPLE;
			}
			if(num_of_cells > 3) {
				t_led_color[3] = LED_PURPLE;
			}
			if(num_of_cells > 4) {
				t_led_color[4] = LED_PURPLE;
			}
			t_led_color[5] = LED_OFF;
			t_led_color[6] = LED_OFF;
			t_led_color[7] = LED_OFF;
			t_led_blink = LED_BLINK;
		} else {
			t_led_color[0] = led_color_1;
			t_led_color[1] = led_color_2;
			t_led_color[2] = led_color_3;
			t_led_color[3] = led_color_4;
			t_led_color[4] = led_color_5;
			t_led_color[5] = led_color_6;
			t_led_color[6] = led_color_7;
			t_led_color[7] = led_color_8;
			t_led_blink = led_blink;
		}
		led_thread_ready = false;
	}

	if (led_thread_runcount & 1) {
		if (t_led_blink)
			setLEDColor(LED_OFF);
		led_interval = LED_OFFTIME;
	} else {
		setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
		//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
		led_interval = LED_ONTIME;
	}

	if (led_thread_runcount == 15) {
		/* obtained data for the first file descriptor */
		struct vehicle_status_s vehicle_status_raw;
		struct vehicle_gps_position_s vehicle_gps_position_raw;

		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));

		bool new_data_vehicle_status;
		bool new_data_vehicle_gps_position;

		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);

		if (new_data_vehicle_status) {
			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
			no_data_vehicle_status = 0;
		} else {
			no_data_vehicle_status++;
			if(no_data_vehicle_status >= 3)
				no_data_vehicle_status = 3;
		}

		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);

		if (new_data_vehicle_gps_position) {
			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
			no_data_vehicle_gps_position = 0;
		} else {
			no_data_vehicle_gps_position++;
			if(no_data_vehicle_gps_position >= 3)
				no_data_vehicle_gps_position = 3;
		}



		/* get number of used satellites in navigation */
		num_of_used_sats = 0;
		//for(int satloop=0; satloop<20; satloop++) {
		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
				num_of_used_sats++;
			}
		}

		if(new_data_vehicle_status || no_data_vehicle_status < 3){
			if(num_of_cells == 0) {
				/* looking for lipo cells that are connected */
				printf("<blinkm> checking cells\n");
				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
				}
				printf("<blinkm> cells found:%u\n", num_of_cells);

			} else {
				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
					/* LED Pattern for battery low warning */
					led_color_1 = LED_YELLOW;
					led_color_2 = LED_YELLOW;
					led_color_3 = LED_YELLOW;
					led_color_4 = LED_YELLOW;
					led_color_5 = LED_YELLOW;
					led_color_6 = LED_YELLOW;
					led_color_7 = LED_YELLOW;
					led_color_8 = LED_YELLOW;
					led_blink = LED_BLINK;

				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
					/* LED Pattern for battery critical alerting */
					led_color_1 = LED_RED;
					led_color_2 = LED_RED;
					led_color_3 = LED_RED;
					led_color_4 = LED_RED;
					led_color_5 = LED_RED;
					led_color_6 = LED_RED;
					led_color_7 = LED_RED;
					led_color_8 = LED_RED;
					led_blink = LED_BLINK;

				} else {
					/* no battery warnings here */

					if(vehicle_status_raw.flag_system_armed == false) {
						/* system not armed */
						led_color_1 = LED_RED;
						led_color_2 = LED_RED;
						led_color_3 = LED_RED;
						led_color_4 = LED_RED;
						led_color_5 = LED_RED;
						led_color_6 = LED_RED;
						led_color_7 = LED_RED;
						led_color_8 = LED_RED;
						led_blink = LED_NOBLINK;

					} else {
						/* armed system - initial led pattern */
						led_color_1 = LED_RED;
						led_color_2 = LED_RED;
						led_color_3 = LED_RED;
						led_color_4 = LED_OFF;
						led_color_5 = LED_OFF;
						led_color_6 = LED_OFF;
						led_color_7 = LED_OFF;
						led_color_8 = LED_OFF;
						led_blink = LED_BLINK;

						/* handle 4th led - flightmode indicator */
						switch((int)vehicle_status_raw.flight_mode) {
							case VEHICLE_FLIGHT_MODE_MANUAL:
								led_color_4 = LED_AMBER;
								break;

							case VEHICLE_FLIGHT_MODE_STAB:
								led_color_4 = LED_YELLOW;
								break;

							case VEHICLE_FLIGHT_MODE_HOLD:
								led_color_4 = LED_BLUE;
								break;

							case VEHICLE_FLIGHT_MODE_AUTO:
								led_color_4 = LED_GREEN;
								break;
						}

						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
							/* handling used sat´s */
							if(num_of_used_sats >= 7) {
								led_color_1 = LED_OFF;
								led_color_2 = LED_OFF;
								led_color_3 = LED_OFF;
							} else if(num_of_used_sats == 6) {
								led_color_2 = LED_OFF;
								led_color_3 = LED_OFF;
							} else if(num_of_used_sats == 5) {
								led_color_3 = LED_OFF;
							}

						} else {
							/* no vehicle_gps_position data */
							led_color_1 = LED_WHITE;
							led_color_2 = LED_WHITE;
							led_color_3 = LED_WHITE;

						}

					}
				}
			}
		} else {
			/* LED Pattern for general Error - no vehicle_status can retrieved */
			led_color_1 = LED_WHITE;
			led_color_2 = LED_WHITE;
			led_color_3 = LED_WHITE;
			led_color_4 = LED_WHITE;
			led_color_5 = LED_WHITE;
			led_color_6 = LED_WHITE;
			led_color_7 = LED_WHITE;
			led_color_8 = LED_WHITE;
			led_blink = LED_BLINK;

		}

		/*
		printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
		vehicle_status_raw.voltage_battery,
		vehicle_status_raw.flag_system_armed,
		vehicle_status_raw.flight_mode,
		num_of_cells,
		no_data_vehicle_status,
		no_data_vehicle_gps_position,
		num_of_used_sats,
		vehicle_gps_position_raw.fix_type,
		vehicle_gps_position_raw.satellites_visible);
		*/

		led_thread_runcount=0;
		led_thread_ready = true;
		led_interval = LED_OFFTIME;

		if(detected_cells_runcount < 4){
			detected_cells_runcount++;
		} else {
			detected_cells_blinked = true;
		}

	} else {
		led_thread_runcount++;
	}

	if(systemstate_run == true) {
		/* re-queue ourselves to run again later */
		work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
	} else {
		stop_script();
		set_rgb(0,0,0);
	}
}
/*
 * 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 */

    // print text
    printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
    fflush(stdout);

    int overloadcounter = 19;

    /* Initialize filter */
    attitudeKalmanfilter_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));
    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));
    struct vehicle_vicon_position_s vicon_pos;  // Added by Ross Allen
    memset(&vicon_pos, 0, sizeof(vicon_pos));

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

    /* Vicon parameters - Added by Ross Allen */
    bool vicon_valid = false;
    //~ static const hrt_abstime vicon_topic_timeout = 500000;		// vicon topic timeout = 0.25s

    /* 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 vicon position */
    int vehicle_vicon_position_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); // Added by Ross Allen

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

    int loopcounter = 0;

    thread_running = true;

    /* advertise debug value */
    // struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
    // orb_advert_t pub_dbg = -1;

    /* keep track of sensor updates */
    uint64_t sensor_last_timestamp[3] = {0, 0, 0};

    struct attitude_estimator_ekf_params ekf_params;

    struct attitude_estimator_ekf_param_handles ekf_param_handles;

    /* 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();

    /* 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);

        /* Added by Ross Allen */
        //~ hrt_abstime t = hrt_absolute_time();
        bool updated;

        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) {
                fprintf(stderr,
                        "[att ekf] WARNING: Not getting sensors - sensor app running?\n");
            }

        } 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;
                    }

                    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;
                    }

                    attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
                                         euler, Rot_matrix, x_aposteriori, P_aposteriori);

                    /* 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;

                    /* Check Vicon for attitude data*/
                    /* Added by Ross Allen */
                    orb_check(vehicle_vicon_position_sub, &updated);

                    if (updated) {
                        orb_copy(ORB_ID(vehicle_vicon_position), vehicle_vicon_position_sub, &vicon_pos);
                        vicon_valid = vicon_pos.valid;
                    }

                    //~ if (vicon_valid && (t > (vicon_pos.timestamp + vicon_topic_timeout))) {
                    //~ vicon_valid = false;
                    //~ warnx("VICON timeout");
                    //~ }

                    /* send out */
                    att.timestamp = raw.timestamp;

                    att.roll = euler[0];
                    att.pitch = euler[1];
                    att.yaw = euler[2] + mag_decl;

                    /* Use vicon yaw if valid and just overwrite*/
                    /* Added by Ross Allen */
                    if(vicon_valid) {
                        att.yaw = vicon_pos.yaw;
                    }

                    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;

                    /* copy rotation matrix */
                    memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
                    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;
}
Exemple #17
0
void
BottleDrop::task_main()
{

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	mavlink_log_info(_mavlink_fd, "[bottle_drop] started");

	_command_sub = orb_subscribe(ORB_ID(vehicle_command));
	_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));

	bool updated = false;

	float z_0;		// ground properties
	float turn_radius;	// turn radius of the UAV
	float precision;	// Expected precision of the UAV

	float ground_distance = _alt_clearance;		// Replace by closer estimate in loop

	// constant
	float g = CONSTANTS_ONE_G;               		// constant of gravity [m/s^2]
	float m = 0.5f;                		// mass of bottle [kg]
	float rho = 1.2f;              		// air density [kg/m^3]
	float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
	float dt_freefall_prediction = 0.01f;   // step size of the free fall prediction [s]

	// Has to be estimated by experiment
	float cd = 0.86f;              	// Drag coefficient for a cylinder with a d/l ratio of 1/3 []
	float t_signal =
		0.084f;	// Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
	float t_door =
		0.7f;		// The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]


	// Definition
	float h_0;						// height over target
	float az;                 				// acceleration in z direction[m/s^2]
	float vz; 						// velocity in z direction [m/s]
	float z; 						// fallen distance [m]
	float h; 						// height over target [m]
	float ax;						// acceleration in x direction [m/s^2]
	float vx;						// ground speed in x direction [m/s]
	float x;					        // traveled distance in x direction [m]
	float vw;                 				// wind speed [m/s]
	float vrx;						// relative velocity in x direction [m/s]
	float v;						// relative speed vector [m/s]
	float Fd;						// Drag force [N]
	float Fdx;						// Drag force in x direction [N]
	float Fdz;						// Drag force in z direction [N]
	float x_drop, y_drop;					// coordinates of the drop point in reference to the target (projection of NED)
	float x_t, y_t;						// coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
	float x_l, y_l;						// local position in projected coordinates
	float x_f, y_f;						// to-be position of the UAV after dt_runs seconds in projected coordinates
	double x_f_NED, y_f_NED;				// to-be position of the UAV after dt_runs seconds in NED
	float distance_open_door;				// The distance the UAV travels during its doors open [m]
	float approach_error = 0.0f;				// The error in radians between current ground vector and desired ground vector
	float distance_real = 0;				// The distance between the UAVs position and the drop point [m]
	float future_distance = 0;				// The distance between the UAVs to-be position and the drop point [m]

	unsigned counter = 0;

	param_t param_gproperties = param_find("BD_GPROPERTIES");
	param_t param_turn_radius = param_find("BD_TURNRADIUS");
	param_t param_precision = param_find("BD_PRECISION");
	param_t param_cd = param_find("BD_OBJ_CD");
	param_t param_mass = param_find("BD_OBJ_MASS");
	param_t param_surface = param_find("BD_OBJ_SURFACE");


	param_get(param_precision, &precision);
	param_get(param_turn_radius, &turn_radius);
	param_get(param_gproperties, &z_0);
	param_get(param_cd, &cd);
	param_get(param_mass, &m);
	param_get(param_surface, &A);

	int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));

	struct parameter_update_s update;
	memset(&update, 0, sizeof(update));
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));

	struct mission_item_s flight_vector_s {};
	struct mission_item_s flight_vector_e {};

	flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
	flight_vector_s.acceptance_radius = 50; // TODO: make parameter
	flight_vector_s.autocontinue = true;
	flight_vector_s.altitude_is_relative = false;

	flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
	flight_vector_e.acceptance_radius = 50; // TODO: make parameter
	flight_vector_e.autocontinue = true;
	flight_vector_s.altitude_is_relative = false;

	struct wind_estimate_s wind;

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

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

	// Whatever state the bay is in, we want it closed on startup
	lock_release();
	close_bay();

	while (!_task_should_exit) {

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

		/* 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;
		}

		/* vehicle commands updated */
		if (fds[0].revents & POLLIN) {
			orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
			handle_command(&_command);
		}

		orb_check(vehicle_global_position_sub, &updated);
		if (updated) {
			/* copy global position */
			orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
		}

		if (_global_pos.timestamp == 0) {
			continue;
		}

		const unsigned sleeptime_us = 9500;

		hrt_abstime last_run = hrt_absolute_time();
		float dt_runs = sleeptime_us / 1e6f;

		// switch to faster updates during the drop
		while (_drop_state > DROP_STATE_INIT) {

			// Get wind estimate
			orb_check(_wind_estimate_sub, &updated);
			if (updated) {
				orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
			}

			// Get vehicle position
			orb_check(vehicle_global_position_sub, &updated);
			if (updated) {
				// copy global position
				orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
			}

			// Get parameter updates
			orb_check(parameter_update_sub, &updated);
			if (updated) {
				// copy global position
				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);

				// update all parameters
				param_get(param_gproperties, &z_0);
				param_get(param_turn_radius, &turn_radius);
				param_get(param_precision, &precision);
			}

			orb_check(_command_sub, &updated);
			if (updated) {
				orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
				handle_command(&_command);
			}


			float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
			float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
			ground_distance = _global_pos.alt - _target_position.alt;

			// Distance to drop position and angle error to approach vector
			// are relevant in all states greater than target valid (which calculates these positions)
			if (_drop_state > DROP_STATE_TARGET_VALID) {
				distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));

				float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
				float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);

				approach_error = _wrap_pi(ground_direction - approach_direction);

				if (counter % 90 == 0) {
					mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
				}
			}

			switch (_drop_state) {
				case DROP_STATE_INIT:
					// do nothing
					break;

				case DROP_STATE_TARGET_VALID:
				{

					az = g;							// acceleration in z direction[m/s^2]
					vz = 0; 						// velocity in z direction [m/s]
					z = 0; 							// fallen distance [m]
					h_0 = _global_pos.alt - _target_position.alt; 		// height over target at start[m]
					h = h_0;						// height over target [m]
					ax = 0;							// acceleration in x direction [m/s^2]
					vx = groundspeed_body;// XXX project					// ground speed in x direction [m/s]
					x = 0;							// traveled distance in x direction [m]
					vw = 0;							// wind speed [m/s]
					vrx = 0;						// relative velocity in x direction [m/s]
					v = groundspeed_body;					// relative speed vector [m/s]
					Fd = 0;							// Drag force [N]
					Fdx = 0;						// Drag force in x direction [N]
					Fdz = 0;						// Drag force in z direction [N]


					// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
					while (h > 0.05f) {
						// z-direction
						vz = vz + az * dt_freefall_prediction;
						z = z + vz * dt_freefall_prediction;
						h = h_0 - z;

						// x-direction
						vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
						vx = vx + ax * dt_freefall_prediction;
						x = x + vx * dt_freefall_prediction;
						vrx = vx + vw;

						// drag force
						v = sqrtf(vz * vz + vrx * vrx);
						Fd = 0.5f * rho * A * cd * (v * v);
						Fdx = Fd * vrx / v;
						Fdz = Fd * vz / v;

						// acceleration
						az = g - Fdz / m;
						ax = -Fdx / m;
					}

					// compute drop vector
					x = groundspeed_body * t_signal + x;

					x_t = 0.0f;
					y_t = 0.0f;

					float wind_direction_n, wind_direction_e;

					if (windspeed_norm < 0.5f) {	// If there is no wind, an arbitrarily direction is chosen
						wind_direction_n = 1.0f;
						wind_direction_e = 0.0f;

					} else {
						wind_direction_n = wind.windspeed_north / windspeed_norm;
						wind_direction_e = wind.windspeed_east / windspeed_norm;
					}

					x_drop = x_t + x * wind_direction_n;
					y_drop = y_t + x * wind_direction_e;
					map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
					_drop_position.alt = _target_position.alt + _alt_clearance;

					// Compute flight vector
					map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
								 &(flight_vector_s.lat), &(flight_vector_s.lon));
					flight_vector_s.altitude = _drop_position.alt;
					map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
								 &flight_vector_e.lat, &flight_vector_e.lon);
					flight_vector_e.altitude = _drop_position.alt;

					// Save WPs in datamanager
					const ssize_t len = sizeof(struct mission_item_s);

					if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
						warnx("ERROR: could not save onboard WP");
					}

					if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
						warnx("ERROR: could not save onboard WP");
					}

					_onboard_mission.count = 2;
					_onboard_mission.current_seq = 0;

					if (_onboard_mission_pub > 0) {
						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);

					} else {
						_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
					}

					float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
					mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));

					_drop_state = DROP_STATE_TARGET_SET;
				}
				break;

				case DROP_STATE_TARGET_SET:
				{
					float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);

					if (distance_wp2 < distance_real) {
						_onboard_mission.current_seq = 0;
						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
					} else {

						// We're close enough - open the bay
						distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));

						if (isfinite(distance_real) && distance_real < distance_open_door &&
							fabsf(approach_error) < math::radians(20.0f)) {
							open_bay();
							_drop_state = DROP_STATE_BAY_OPEN;
							mavlink_log_info(_mavlink_fd, "#audio: opening bay");
						}
					}
				}
				break;

				case DROP_STATE_BAY_OPEN:
					{
						if (_drop_approval) {
							map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
							x_f = x_l + _global_pos.vel_n * dt_runs;
							y_f = y_l + _global_pos.vel_e * dt_runs;
							map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
							future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);

							if (isfinite(distance_real) &&
								(distance_real < precision) && ((distance_real < future_distance))) {
									drop();
									_drop_state = DROP_STATE_DROPPED;
									mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
							} else {

								float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);

								if (distance_wp2 < distance_real) {
									_onboard_mission.current_seq = 0;
									orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
								}
							}
						}
					}
					break;

				case DROP_STATE_DROPPED:
					/* 2s after drop, reset and close everything again */
					if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
						_drop_state = DROP_STATE_INIT;
						_drop_approval = false;
						lock_release();
						close_bay();
						mavlink_log_info(_mavlink_fd, "#audio: closing bay");

						// remove onboard mission
						_onboard_mission.current_seq = -1;
						_onboard_mission.count = 0;
						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
					}
					break;

				case DROP_STATE_BAY_CLOSED:
					// do nothing
					break;
			}

			counter++;

			// update_actuators();

			// run at roughly 100 Hz
			usleep(sleeptime_us);

			dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
			last_run = hrt_absolute_time();
		}
	}

	warnx("exiting.");

	_main_task = -1;
	_exit(0);
}
int quad_commander_thread_main(int argc, char *argv[]) {
        warnx("[quad_commander] has begun");

        /**
         * Subscriptions
         */
        struct quad_swarm_cmd_s swarm_cmd;
        struct quad_mode_s state;
        struct vehicle_status_s v_status;

        memset(&swarm_cmd, 0, sizeof(swarm_cmd));
        memset(&state, 0, sizeof(state));
        memset(&v_status, 0, sizeof(v_status));

        int swarm_cmd_sub = 0;
        int state_sub = 0;
        int v_status_sub = 0;

        swarm_cmd_sub = orb_subscribe(ORB_ID(quad_swarm_cmd));
        state_sub = orb_subscribe(ORB_ID(quad_mode));
        v_status_sub = orb_subscribe(ORB_ID(vehicle_status));

        /**
         * Topics to be published on
         */
        struct quad_mode_s mode;
        orb_advert_t mode_pub = orb_advertise(ORB_ID(quad_mode), &mode);
        memset(&mode, 0, sizeof(mode));

	param_t param_ptr = param_find("MAV_SYS_ID");
	param_get(param_ptr, &system_id);

        mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
        mavlink_log_info(mavlink_fd, "[QC%i] started", system_id);

        struct pollfd fd_cmd[1];
        fd_cmd[0].fd = swarm_cmd_sub;
        fd_cmd[0].events = POLLIN;

        struct pollfd fd_state;
        fd_state.fd = state_sub;
        fd_state.events = POLLIN;

        /* Initial state of the quadrotor; operations always start from the ground */
        state.current_state = QUAD_STATE_GROUNDED;
        mode.current_state = QUAD_STATE_GROUNDED;
        orb_publish(ORB_ID(quad_mode), mode_pub, &mode);

        bool transition_error  = false;

        while ( !thread_should_exit ) {
                bool v_status_updated;
                orb_check(v_status_sub, &v_status_updated);
                if ( v_status_updated )
                        orb_copy(ORB_ID(vehicle_status), v_status_sub, &v_status);

                if ( v_status.arming_state == ARMING_STATE_STANDBY ) {
                        state.current_state = QUAD_STATE_GROUNDED;
                        mode.cmd = QUAD_CMD_PENDING;
                        mode.current_state = QUAD_STATE_GROUNDED;
                        orb_publish(ORB_ID(quad_mode), mode_pub, &mode);
                        swarm_cmd.cmd_id = (float)0;
                }

                if ( (v_status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) &&
                     (state.current_state != QUAD_STATE_GROUNDED) ) {

                        low_battery = true;
                        mavlink_log_critical(mavlink_fd, "[QC%i] Battery lavel low!", system_id);
                        emergency_land( &state, &mode, &mode_pub, &state_sub, &swarm_cmd );

                } else {
                        low_battery = false;
                }

                int ret_state = poll(&fd_state, 1, 1);
                if ( ret_state < 0 ) {
                        warnx("poll cmd error");
                } else if ( ret_state == 0 ) {
                        /* nothing happened */
                } else if ( fd_state.revents & POLLIN ) {
                        orb_copy(ORB_ID(quad_mode), state_sub, &state);
                } else {
                        /* nothing happened */
                }

                if ( state.error == true ) {
                        int ret_value = emergency_land( &state, &mode, &mode_pub, &state_sub, &swarm_cmd );
                	mavlink_log_info(mavlink_fd, "[QC%i] emergency landing", system_id);
                        error_msg( ret_value, &transition_error );
                }

                int ret_cmd = poll(fd_cmd, 1, 250);
                if (ret_cmd < 0) {
                        warnx("poll cmd error");
                } else if (ret_cmd == 0) {
                        /* no return value - nothing has happened */
                } else if (fd_cmd[0].revents & POLLIN) {
                        orb_copy(ORB_ID(quad_swarm_cmd), swarm_cmd_sub, &swarm_cmd);

                        if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_TAKEOFF ) {
                                mavlink_log_info(mavlink_fd, "[QC%i] Takeoff transition begun!", system_id);
                                int ret_value = take_off( &state, &mode, &mode_pub, &state_sub );
                                error_msg( ret_value, &transition_error );

                        } else if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_LAND ) {
                                mavlink_log_info(mavlink_fd, "[QC%i] Landing transition begun!", system_id);
                                int ret_value = land( &state, &mode, &mode_pub, &state_sub, &transition_error );
                                error_msg( ret_value, &transition_error );

                        } else if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_START_SWARM ) {
                                mavlink_log_info(mavlink_fd, "[QC%i] Swarming transition begun!", system_id);
                                int ret_value = start_swarm( &state, &mode, &mode_pub, &state_sub );
                                error_msg( ret_value, &transition_error );

                        } else if ( swarm_cmd.cmd_id == (enum QUAD_MSG_CMD)QUAD_MSG_CMD_STOP_SWARM ) {
                                mavlink_log_info(mavlink_fd, "[QC%i] Stop swarming transition begun!", system_id);
                                int ret_value = stop_swarm( &state, &mode, &mode_pub, &state_sub );
                                error_msg( ret_value, &transition_error );

                        } else {
                                /* Nothing to do */
                        }

                } else {
                        /* nothing happened */
                }
        }
}
Exemple #19
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));
	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
	_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);
	fw_pos_ctrl_status_update();
	params_update();

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

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _vehicle_command_sub;
	fds[1].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) {
				global_pos_available_once = false;
				PX4_WARN("navigator: global position timeout");
			}
			/* Let the loop run anyway, don't do `continue` here. */

		} 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;
		} else {
			/* success, global pos was available */
			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(_fw_pos_ctrl_status_sub, &updated);
		if (updated) {
			fw_pos_ctrl_status_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 = get_global_position()->yaw;
				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 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : 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;
			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
				struct position_setpoint_triplet_s *rep = get_takeoff_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = get_global_position()->yaw;
				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_TAKEOFF;
				rep->current.yaw = cmd.param4;

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
				} else {
					// If one of them is non-finite, reset both
					rep->current.lat = NAN;
					rep->current.lon = NAN;
				}

				rep->current.alt = cmd.param7;

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

			} else 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, _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 (_fw_pos_ctrl_status.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;
}
Exemple #20
0
// update all estimator topics and write them to log file
void Ekf2Replay::logIfUpdated()
{
	bool updated = false;

	// update attitude
	struct vehicle_attitude_s att = {};
	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

	// if the timestamp of the attitude is zero, then this means that the ekf did not
	// do an update so we can ignore this message and just continue
	if (att.timestamp == 0) {
		return;
	}

	memset(&log_message.body.att.q_w, 0, sizeof(log_ATT_s));

	log_message.type = LOG_ATT_MSG;
	log_message.head1 = HEAD_BYTE1;
	log_message.head2 = HEAD_BYTE2;
	log_message.body.att.q_w = att.q[0];
	log_message.body.att.q_x = att.q[1];
	log_message.body.att.q_y = att.q[2];
	log_message.body.att.q_z = att.q[3];
	log_message.body.att.roll = atan2f(2 * (att.q[0] * att.q[1] + att.q[2] * att.q[3]),
					   1 - 2 * (att.q[1] * att.q[1] + att.q[2] * att.q[2]));
	log_message.body.att.pitch = asinf(2 * (att.q[0] * att.q[2] - att.q[3] * att.q[1]));
	log_message.body.att.yaw = atan2f(2 * (att.q[0] * att.q[3] + att.q[1] * att.q[2]),
					  1 - 2 * (att.q[2] * att.q[2] + att.q[3] * att.q[3]));
	log_message.body.att.roll_rate = att.rollspeed;
	log_message.body.att.pitch_rate = att.pitchspeed;
	log_message.body.att.yaw_rate = att.yawspeed;

	writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length);

	// update local position
	orb_check(_lpos_sub, &updated);

	if (updated) {
		struct vehicle_local_position_s lpos = {};
		orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &lpos);

		log_message.type = LOG_LPOS_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		log_message.body.lpos.x = lpos.x;
		log_message.body.lpos.y = lpos.y;
		log_message.body.lpos.z = lpos.z;
		log_message.body.lpos.ground_dist = lpos.dist_bottom;
		log_message.body.lpos.ground_dist_rate = lpos.dist_bottom_rate;
		log_message.body.lpos.vx = lpos.vx;
		log_message.body.lpos.vy = lpos.vy;
		log_message.body.lpos.vz = lpos.vz;
		log_message.body.lpos.ref_lat = lpos.ref_lat * 1e7;
		log_message.body.lpos.ref_lon = lpos.ref_lon * 1e7;
		log_message.body.lpos.ref_alt = lpos.ref_alt;
		log_message.body.lpos.pos_flags = (lpos.xy_valid ? 1 : 0) |
						  (lpos.z_valid ? 2 : 0) |
						  (lpos.v_xy_valid ? 4 : 0) |
						  (lpos.v_z_valid ? 8 : 0) |
						  (lpos.xy_global ? 16 : 0) |
						  (lpos.z_global ? 32 : 0);
		log_message.body.lpos.ground_dist_flags = (lpos.dist_bottom_valid ? 1 : 0);
		log_message.body.lpos.eph = lpos.eph;
		log_message.body.lpos.epv = lpos.epv;

		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_LPOS_MSG].length);
	}

	// update estimator status
	orb_check(_estimator_status_sub, &updated);

	if (updated) {
		struct estimator_status_s est_status = {};
		orb_copy(ORB_ID(estimator_status), _estimator_status_sub, &est_status);
		unsigned maxcopy0 = (sizeof(est_status.states) < sizeof(log_message.body.est0.s)) ? sizeof(est_status.states) : sizeof(
					    log_message.body.est0.s);
		log_message.type = LOG_EST0_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		memset(&(log_message.body.est0.s), 0, sizeof(log_message.body.est0));
		memcpy(&(log_message.body.est0.s), est_status.states, maxcopy0);
		log_message.body.est0.n_states = est_status.n_states;
		log_message.body.est0.nan_flags = est_status.nan_flags;
		log_message.body.est0.fault_flags = est_status.filter_fault_flags;
		log_message.body.est0.timeout_flags = est_status.timeout_flags;
		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST0_MSG].length);

		log_message.type = LOG_EST1_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		unsigned maxcopy1 = ((sizeof(est_status.states) - maxcopy0) < sizeof(log_message.body.est1.s)) ? (sizeof(
					    est_status.states) - maxcopy0) : sizeof(log_message.body.est1.s);
		memset(&(log_message.body.est1.s), 0, sizeof(log_message.body.est1.s));
		memcpy(&(log_message.body.est1.s), ((char *)est_status.states) + maxcopy0, maxcopy1);
		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST1_MSG].length);

		log_message.type = LOG_EST2_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		unsigned maxcopy2 = (sizeof(est_status.covariances) < sizeof(log_message.body.est2.cov)) ? sizeof(
					    est_status.covariances) : sizeof(log_message.body.est2.cov);
		memset(&(log_message.body.est2.cov), 0, sizeof(log_message.body.est2.cov));
		memcpy(&(log_message.body.est2.cov), est_status.covariances, maxcopy2);
		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST2_MSG].length);

		log_message.type = LOG_EST3_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		unsigned maxcopy3 = ((sizeof(est_status.covariances) - maxcopy2) < sizeof(log_message.body.est3.cov)) ? (sizeof(
					    est_status.covariances) - maxcopy2) : sizeof(log_message.body.est3.cov);
		memset(&(log_message.body.est3.cov), 0, sizeof(log_message.body.est3.cov));
		memcpy(&(log_message.body.est3.cov), ((char *)est_status.covariances) + maxcopy2, maxcopy3);
		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST3_MSG].length);

	}

	// update ekf2 innovations
	orb_check(_innov_sub, &updated);

	if (updated) {
		struct ekf2_innovations_s innov = {};
		orb_copy(ORB_ID(ekf2_innovations), _innov_sub, &innov);
		memset(&log_message.body.innov.s, 0, sizeof(log_message.body.innov.s));

		log_message.type = LOG_EST4_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;

		for (unsigned i = 0; i < 6; i++) {
			log_message.body.innov.s[i] = innov.vel_pos_innov[i];
			log_message.body.innov.s[i + 6] = innov.vel_pos_innov_var[i];
		}

		for (unsigned i = 0; i < 3; i++) {
			log_message.body.innov.s[i + 12] = innov.output_tracking_error[i];
		}

		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST4_MSG].length);

		log_message.type = LOG_EST5_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		memset(&(log_message.body.innov2.s), 0, sizeof(log_message.body.innov2.s));

		for (unsigned i = 0; i < 3; i++) {
			log_message.body.innov2.s[i] = innov.mag_innov[i];
			log_message.body.innov2.s[i + 3] = innov.mag_innov_var[i];
		}

		log_message.body.innov2.s[6] = innov.heading_innov;
		log_message.body.innov2.s[7] = innov.heading_innov_var;
		log_message.body.innov2.s[8] = innov.airspeed_innov;
		log_message.body.innov2.s[9] = innov.airspeed_innov_var;

		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST5_MSG].length);

		// optical flow innovations and innovation variances
		log_message.type = LOG_EST6_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		memset(&(log_message.body.innov3.s), 0, sizeof(log_message.body.innov3.s));

		for (unsigned i = 0; i < 2; i++) {
			log_message.body.innov3.s[i] = innov.flow_innov[i];
			log_message.body.innov3.s[i + 2] = innov.flow_innov_var[i];
		}

		log_message.body.innov3.s[4] = innov.hagl_innov;
		log_message.body.innov3.s[5] = innov.hagl_innov_var;
		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST6_MSG].length);
	}

	// update control state
	orb_check(_control_state_sub, &updated);

	if (updated) {
		struct control_state_s control_state = {};
		orb_copy(ORB_ID(control_state), _control_state_sub, &control_state);
		log_message.type = LOG_CTS_MSG;
		log_message.head1 = HEAD_BYTE1;
		log_message.head2 = HEAD_BYTE2;
		log_message.body.control_state.vx_body = control_state.x_vel;
		log_message.body.control_state.vy_body = control_state.y_vel;
		log_message.body.control_state.vz_body = control_state.z_vel;
		log_message.body.control_state.airspeed = control_state.airspeed;
		log_message.body.control_state.roll_rate = control_state.roll_rate;
		log_message.body.control_state.pitch_rate = control_state.pitch_rate;
		log_message.body.control_state.yaw_rate = control_state.yaw_rate;
		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_CTS_MSG].length);
	}
}
Exemple #21
0
int
nshterm_main(int argc, char *argv[])
{
    if (argc < 2) {
        printf("Usage: nshterm <device>\n");
        exit(1);
    }
    unsigned retries = 0;
    int fd = -1;
    int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
    struct actuator_armed_s armed;
    /* we assume the system does not provide arming status feedback */
    bool armed_updated = false;

    /* try the first 30 seconds or if arming system is ready */
    while ((retries < 300) || armed_updated) {

        /* abort if an arming topic is published and system is armed */
        bool updated = false;
        if (orb_check(armed_fd, &updated)) {
            /* the system is now providing arming status feedback.
             * instead of timing out, we resort to abort bringing
             * up the terminal.
             */
            armed_updated = true;
            orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);

            if (armed.armed) {
                /* this is not an error, but we are done */
                exit(0);
            }
        }

        /* the retries are to cope with the behaviour of /dev/ttyACM0 */
        /* which may not be ready immediately. */
        fd = open(argv[1], O_RDWR);
        if (fd != -1) {
            break;
        }
        usleep(100000);
        retries++;
    }
    if (fd == -1) {
        perror(argv[1]);
        exit(1);
    }

    /* set up the serial port with output processing */
    
    /* Try to set baud rate */
    struct termios uart_config;
    int termios_state;

    /* Back up the original uart configuration to restore it after exit */
    if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
        warnx("ERR get config %s: %d\n", argv[1], termios_state);
        close(fd);
        return -1;
    }

    /* Set ONLCR flag (which appends a CR for every LF) */
    uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);

    if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
        warnx("ERR set config %s\n", argv[1]);
        close(fd);
        return -1;
    }

    /* setup standard file descriptors */
    close(0);
    close(1);
    close(2);
    dup2(fd, 0);
    dup2(fd, 1);
    dup2(fd, 2);

    nsh_consolemain(0, NULL);

    close(fd);

    return OK;
}
Exemple #22
0
void
Mission::on_active()
{
	check_mission_valid(false);

	/* check if anything has changed */
	bool onboard_updated = false;
	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);

	if (onboard_updated) {
		update_onboard_mission();
	}

	bool offboard_updated = false;
	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);

	if (offboard_updated) {
		update_offboard_mission();
	}

	/* reset the current offboard mission if needed */
	if (need_to_reset_mission(true)) {
		reset_offboard_mission(_offboard_mission);
		update_offboard_mission();
		offboard_updated = true;
	}

	/* reset mission items if needed */
	if (onboard_updated || offboard_updated) {
		set_mission_items();
	}

	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
		set_mission_item_reached();

		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
		}

	} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
		altitude_sp_foh_update();

	} else {
		/* if waypoint position reached allow loiter on the setpoint */
		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
			_navigator->set_can_loiter_at_sp(true);
		}
	}

	/* see if we need to update the current yaw heading */
	if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
	     && _param_yawmode.get() < MISSION_YAWMODE_MAX
	     && _mission_type != MISSION_TYPE_NONE)
	    || _navigator->get_vstatus()->is_vtol) {

		heading_sp_update();
	}

	/* check if landing needs to be aborted */
	if ((_mission_item.nav_cmd == NAV_CMD_LAND)
	    && (_navigator->abort_landing())) {

		do_abort_landing();
	}

}
Exemple #23
0
void
Gimbal::cycle()
{
	if (!_initialized) {
		/* get subscription handles */
		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
		_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
		_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
		_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
		_params_sub = orb_subscribe(ORB_ID(parameter_update));

		/* get a publication handle on actuator output topic */
		struct actuator_controls_s zero_report;
		memset(&zero_report, 0, sizeof(zero_report));
		zero_report.timestamp = hrt_absolute_time();
		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);

		if (_actuator_controls_2_topic == nullptr) {
			warnx("advert err");
		}

		_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;

		_initialized = true;
	}

	bool	updated = false;

	perf_begin(_sample_perf);

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;
	float out_mount_mode = 0.0f;

	/* Parameter update */
	bool params_updated;

	orb_check(_params_sub, &params_updated);

	if (params_updated) {
		struct parameter_update_s param_update;
		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update); // XXX: is this actually necessary?

		update_params();
	}

	/* Control mode update */
	bool vehicle_control_mode_updated;

	orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);

	if (vehicle_control_mode_updated) {
		orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
	}

	/* Check attitude */
	struct vehicle_attitude_s att;

	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

	if (_attitude_compensation_roll) {
		roll = 1.0f / M_PI_F * -att.roll;
		updated = true;
	}

	if (_attitude_compensation_pitch) {
		pitch = 1.0f / M_PI_F * -att.pitch;
		updated = true;
	}

	if (_attitude_compensation_yaw) {
		yaw = 1.0f / M_PI_F * att.yaw;
		updated = true;
	}

	/* Check manual input */
	bool manual_updated;

	orb_check(_manual_control_sub, &manual_updated);

	if (manual_updated) {
		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);

		/* only check manual input for mount mode when not in offboard and aux chan is configured */
		if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
			float aux = 0.0f;

			switch (_parameters.aux_mnt_chn) {
			case 1:
				aux = _manual_control.aux1;
				break;

			case 2:
				aux = _manual_control.aux2;
				break;

			case 3:
				aux = _manual_control.aux3;
				break;
			}

			if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
				_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
				updated = true;
			}

			if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
				_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
				updated = true;
			}
		}
	}

	/* Check command input */
	struct vehicle_command_s cmd;

	bool cmd_updated;

	orb_check(_vehicle_command_sub, &cmd_updated);

	if (cmd_updated) {

		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
		    || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {

			_control_cmd = cmd;
			_control_cmd_set = true;

		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {

			_config_cmd = cmd;
			_config_cmd_set = true;

		}

	}

	if (_config_cmd_set) {

		_config_cmd_set = false;

		_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
		_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
		_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;

		/* only check commanded mount mode when in offboard */
		if (_control_mode.flag_control_offboard_enabled &&
		    fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {
			_mount_mode = int(_config_cmd.param1 + 0.5f);
			updated = true;
		}

	}

	if (_control_cmd_set) {

		unsigned mountMode = _control_cmd.param7;
		DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
			     (double)_control_cmd.param1, (double)_control_cmd.param2);

		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
		    mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;

			updated = true;
		}

		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
		    mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();

			roll += gimablDirectionEuler(0);
			pitch += gimablDirectionEuler(1);
			yaw += gimablDirectionEuler(2);

			updated = true;
		}

	}

	/* consider mount mode if parameter is set */
	if (_parameters.use_mnt > 0) {
		switch (_mount_mode) {
		case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
			out_mount_mode = -1.0f;
			roll = 0.0f;
			pitch = 0.0f;
			yaw = 0.0f;
			break;

		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
		case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
			out_mount_mode = 1.0f;
			break;

		default:
			out_mount_mode = -1.0f;
		}
	}

	if (updated) {

		struct actuator_controls_s controls;

		// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);

		/* fill in the final control values */
		controls.timestamp = hrt_absolute_time();
		controls.control[0] = roll;
		controls.control[1] = pitch;
		controls.control[2] = yaw;
		//controls.control[3] = ; // camera shutter
		controls.control[4] = out_mount_mode;

		/* publish it */
		orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);

	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	perf_end(_sample_perf);

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(LPWORK,
		   &_work,
		   (worker_t)&Gimbal::cycle_trampoline,
		   this,
		   USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
/**
 * Mission waypoint manager  main function
 */
void *mission_waypoint_manager_thread_main(void* args)
{
	absolute_time now;
	int updated;
	float turn_distance;

	/* initialize waypoint manager */
	// ************************************* subscribe ******************************************
	global_position_setpoint_pub = orb_advertise (ORB_ID(vehicle_global_position_setpoint));
	if (global_position_setpoint_pub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_setpoint topic\n");
		exit (-1);
	}

	global_position_set_triplet_pub = orb_advertise (ORB_ID(vehicle_global_position_set_triplet));
	if (global_position_set_triplet_pub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_set_triplet topic\n");
		exit (-1);
	}

	// ************************************* subscribe ******************************************
	struct vehicle_global_position_s global_pos;
	orb_subscr_t global_pos_sub = orb_subscribe (ORB_ID(vehicle_global_position));
	if (global_pos_sub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to subscribe the vehicle_global_position topic\n");
		exit (-1);
	}

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

	struct navigation_capabilities_s nav_cap;
	orb_subscr_t nav_cap_sub = orb_subscribe (ORB_ID(navigation_capabilities));
	if (nav_cap_sub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to subscribe the navigation_capabilities topic\n");
		exit (-1);
	}

	struct parameter_update_s params;
	orb_subscr_t param_sub = orb_subscribe (ORB_ID(parameter_update));
	if (param_sub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to subscribe the parameter_update topic\n");
		exit (-1);
	}

	orb_subscr_t mission_sub = orb_subscribe (ORB_ID(mission));
	if (mission_sub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to subscribe the mission topic\n");
		exit (-1);
	}

	orb_subscr_t home_sub = orb_subscribe (ORB_ID(home_position));
	if (home_sub == -1)
	{
		fprintf (stderr, "Waypoint manager thread failed to subscribe the home_position topic\n");
		exit (-1);
	}


	/* abort on a nonzero return value from the parameter init */
	if (mission_waypoint_manager_params_init() != 0 || wp_manager_init (mission_sub, home_sub) != 0)
	{
		fprintf (stderr, "Waypoint manager exiting on startup due to an error\n");
		exit (-1);
	}

	turn_distance = (is_rotary_wing)? mission_waypoint_manager_parameters.mr_turn_distance :
									  mission_waypoint_manager_parameters.fw_turn_distance;

	/* welcome user */
	fprintf (stdout, "Waypoint manager started\n");
	fflush(stdout);

	/* waypoint main eventloop */
	while (!_shutdown_all_systems) {
		// wait for the position estimation for a max of 500ms
		updated = orb_poll (ORB_ID(vehicle_global_position), global_pos_sub, 500000);
		if (updated < 0)
		{
			// that's undesiderable but there is not much we can do
			fprintf (stderr, "Waypoint manager thread experienced an error waiting for actuator_controls topic\n");
			continue;
		}
		else if (updated == 0)
		{
			continue;
		}
		else
			orb_copy (ORB_ID(vehicle_global_position), global_pos_sub, (void *) &global_pos);

		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(navigation_capabilities), nav_cap_sub);
		if (updated)
		{
			orb_copy (ORB_ID(navigation_capabilities), nav_cap_sub, (void *) &nav_cap);
			turn_distance = (nav_cap.turn_distance > 0)? nav_cap.turn_distance : turn_distance;
		}

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

			/* update multirotor_position_control_parameters */
			mission_waypoint_manager_params_update();
		}

		if (!control_flags.flag_control_manual_enabled && control_flags.flag_control_auto_enabled)
		{
			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &g_sp);
			orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
		}

		now = get_absolute_time();

		/* check if waypoint has been reached against the last positions */
		check_waypoints_reached(now, &global_pos, turn_distance);

		/* sleep 25 ms */
		usleep(25000);
	}


	/*
	 * do unsubscriptions
	 */
	if (orb_unsubscribe (ORB_ID(vehicle_global_position), global_pos_sub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_global_position topic\n");

	if (orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_control_flags topic\n");

	if (orb_unsubscribe (ORB_ID(navigation_capabilities), nav_cap_sub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unsubscribe to navigation_capabilities topic\n");

	if (orb_unsubscribe (ORB_ID(parameter_update), param_sub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unsubscribe to parameter_update topic\n");

	if (orb_unsubscribe (ORB_ID(mission), mission_sub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unsubscribe to mission topic\n");

	if (orb_unsubscribe (ORB_ID(home_position), home_sub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unsubscribe to home_position topic\n");

	/*
	 * do unadvertises
	 */
	if (orb_unadvertise (ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unadvertise the global_position_setpoint_pub topic\n");

	if (orb_unadvertise (ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, pthread_self()) < 0)
		fprintf (stderr, "Waypoint manager thread failed to unadvertise the vehicle_global_position_set_triplet topic\n");


	return 0;
}
int test_ppm_loopback(int argc, char *argv[])
{

	int _rc_sub = orb_subscribe(ORB_ID(input_rc));

	int servo_fd, result;
	servo_position_t pos;

	servo_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);

	if (servo_fd < 0) {
		printf("failed opening /dev/pwm_servo\n");
	}

	printf("Servo readback, pairs of values should match defaults\n");

	unsigned servo_count;
	result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);

	if (result != OK) {
		warnx("PWM_SERVO_GET_COUNT");

		(void)close(servo_fd);
		return ERROR;
	}

	for (unsigned i = 0; i < servo_count; i++) {
		result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);

		if (result < 0) {
			printf("failed reading channel %u\n", i);
		}

		//printf("%u: %u %u\n", i, pos, data[i]);

	}

	// /* tell safety that its ok to disable it with the switch */
	// result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
	// if (result != OK)
	// 	warnx("FAIL: PWM_SERVO_SET_ARM_OK");
	//  tell output device that the system is armed (it will output values if safety is off)
	// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
	// if (result != OK)
	// 	warnx("FAIL: PWM_SERVO_ARM");

	int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};


	for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
		result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);

		if (result) {
			(void)close(servo_fd);
			return ERROR;

		} else {
			warnx("channel %d set to %d", i, pwm_values[i]);
		}
	}

	warnx("servo count: %d", servo_count);

	struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};

	for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
		pwm_out.values[i] = pwm_values[i];
		//warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
		pwm_out.channel_count++;
	}

	result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);

	/* give driver 10 ms to propagate */

	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
	struct input_rc_s rc_input;
	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
	usleep(100000);

	/* open PPM input and expect values close to the output values */

	bool rc_updated;
	orb_check(_rc_sub, &rc_updated);

	if (rc_updated) {

		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

		// int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);



		// struct input_rc_s rc;
		// result = read(ppm_fd, &rc, sizeof(rc));

		// if (result != sizeof(rc)) {
		// 	warnx("Error reading RC output");
		// 	(void)close(servo_fd);
		// 	(void)close(ppm_fd);
		// 	return ERROR;
		// }

		/* go and check values */
		for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
			if (abs(rc_input.values[i] - pwm_values[i]) > 10) {
				warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
				(void)close(servo_fd);
				return ERROR;
			}
		}

	} else {
		warnx("failed reading RC input data");
		(void)close(servo_fd);
		return ERROR;
	}

	close(servo_fd);

	warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");

	return 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 0x%08x", 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");
}
Exemple #27
0
void
Gimbal::cycle()
{
	if (!_initialized) {
		/* get a subscription handle on the vehicle command topic */
		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

		/* get a publication handle on actuator output topic */
		struct actuator_controls_s zero_report;
		memset(&zero_report, 0, sizeof(zero_report));
		zero_report.timestamp = hrt_absolute_time();
		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);

		if (_actuator_controls_2_topic < 0) {
			warnx("advert err");
		}

		_initialized = true;
	}

	bool	updated = false;

	perf_begin(_sample_perf);

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;


	if (_attitude_compensation) {
		if (_att_sub < 0) {
			_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
		}

		vehicle_attitude_s att;

		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

		roll = -att.roll;
		pitch = -att.pitch;
		yaw = att.yaw;

		updated = true;
	}

	struct vehicle_command_s cmd;

	bool cmd_updated;

	orb_check(_vehicle_command_sub, &cmd_updated);

	if (cmd_updated) {

		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

		VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7;
		debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2);

		if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {

			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1;
			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2;
			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3;
			
			updated = true;

		}

		if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4};
			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();

			roll += gimablDirectionEuler(0);
			pitch += gimablDirectionEuler(1);
			yaw += gimablDirectionEuler(2);

			updated = true;
		}
	}

	if (updated) {

		struct actuator_controls_s controls;

		// debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);

		/* fill in the final control values */
		controls.timestamp = hrt_absolute_time();
		controls.control[0] = roll;
		controls.control[1] = pitch;
		controls.control[2] = yaw;

		/* publish it */
		orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);

	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	perf_end(_sample_perf);

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(LPWORK,
		   &_work,
		   (worker_t)&Gimbal::cycle_trampoline,
		   this,
		   USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
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,
					     1500,
					     (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)");
}
Exemple #29
0
int UavcanNode::run()
{
	(void)pthread_mutex_lock(&_node_mutex);

	// XXX figure out the output count
	_output_count = 2;

	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
	_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));

	memset(&_outputs, 0, sizeof(_outputs));

	/*
	 * Set up the time synchronization
	 */

	const int slave_init_res = _time_sync_slave.start();

	if (slave_init_res < 0) {
		warnx("Failed to start time_sync_slave");
		_task_should_exit = true;
	}

	/* When we have a system wide notion of time update (i.e the transition from the initial
	 * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
	 * happens, but for now we use adjustUtc with a correction of the hrt so that the
	 * time bases are the same
	 */
	uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time()));
	_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
	_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));



	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);

	if (busevent_fd < 0) {
		warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
		_task_should_exit = true;
	}

	/*
	 * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
	 *     IO multiplexing shall be done here.
	 */

	_node.setModeOperational();

	/*
	 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
	 * Please note that with such multiplexing it is no longer possible to rely only on
	 * the value returned from poll() to detect whether actuator control has timed out or not.
	 * Instead, all ORB events need to be checked individually (see below).
	 */
	add_poll_fd(busevent_fd);

	/*
	 * setup poll to look for actuator direct input if we are
	 * subscribed to the topic
	 */
	if (_actuator_direct_sub != -1) {
		_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
	}

	while (!_task_should_exit) {

		switch (_fw_server_action) {
		case Start:
			_fw_server_status =  start_fw_server();
			break;

		case Stop:
			_fw_server_status = stop_fw_server();
			break;

		case CheckFW:
			_fw_server_status = request_fw_check();
			break;

		case None:
		default:
			break;
		}

		// update actuator controls subscriptions if needed
		if (_groups_subscribed != _groups_required) {
			subscribe();
			_groups_subscribed = _groups_required;
		}

		// Mutex is unlocked while the thread is blocked on IO multiplexing
		(void)pthread_mutex_unlock(&_node_mutex);

		perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake

		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);

		perf_begin(_perfcnt_esc_mixer_total_elapsed);

		(void)pthread_mutex_lock(&_node_mutex);

		node_spin_once();  // Non-blocking

		bool new_output = false;

		// this would be bad...
		if (poll_ret < 0) {
			DEVICE_LOG("poll error %d", errno);
			continue;

		} else {
			// get controls for required topics
			bool controls_updated = false;

			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
				if (_control_subs[i] > 0) {
					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
						controls_updated = true;
						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
					}
				}
			}

			/*
			  see if we have any direct actuator updates
			 */
			if (_actuator_direct_sub != -1 &&
			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
			    !_test_in_progress) {
				if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
					_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
				}

				memcpy(&_outputs.output[0], &_actuator_direct.values[0],
				       _actuator_direct.nvalues * sizeof(float));
				_outputs.noutputs = _actuator_direct.nvalues;
				new_output = true;
			}

			// can we mix?
			if (_test_in_progress) {
				memset(&_outputs, 0, sizeof(_outputs));

				if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
					_outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
					_outputs.noutputs = _test_motor.motor_number + 1;
				}

				new_output = true;

			} else if (controls_updated && (_mixers != nullptr)) {

				// XXX one output group has 8 outputs max,
				// but this driver could well serve multiple groups.
				unsigned num_outputs_max = 8;

				// Do mixing
				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL);

				new_output = true;
			}
		}

		if (new_output) {
			// iterate actuators, checking for valid values
			for (uint8_t i = 0; i < _outputs.noutputs; i++) {
				// last resort: catch NaN, INF and out-of-band errors
				if (!isfinite(_outputs.output[i])) {
					/*
					 * Value is NaN, INF or out of band - set to the minimum value.
					 * This will be clearly visible on the servo status and will limit the risk of accidentally
					 * spinning motors. It would be deadly in flight.
					 */
					_outputs.output[i] = -1.0f;
				}

				// never go below min
				if (_outputs.output[i] < -1.0f) {
					_outputs.output[i] = -1.0f;
				}

				// never go above max
				if (_outputs.output[i] > 1.0f) {
					_outputs.output[i] = 1.0f;
				}
			}

			// Output to the bus
			_outputs.timestamp = hrt_absolute_time();
			perf_begin(_perfcnt_esc_mixer_output_elapsed);
			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
			perf_end(_perfcnt_esc_mixer_output_elapsed);
		}


		// Check motor test state
		bool updated = false;
		orb_check(_test_motor_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);

			// Update the test status and check that we're not locked down
			_test_in_progress = (_test_motor.value > 0);
			_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
		}

		// Check arming state
		orb_check(_armed_sub, &updated);

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

			// Update the armed status and check that we're not locked down and motor
			// test is not running
			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;

			arm_actuators(set_armed);
		}
	}

	(void)::close(busevent_fd);

	teardown();
	warnx("exiting.");

	exit(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 unsigned 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 (unsigned int i = 0; i < queue_size - 2; ++i) {
		t.val = i;
		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
	}

	for (unsigned 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 (unsigned int i = 0; i < queue_size + overflow_by; ++i) {
		t.val = i;
		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
	}

	for (unsigned 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 (unsigned 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");
}