int uORBTest::UnitTest::test_unadvertise()
{
	test_note("Testing unadvertise");

	//we still have the advertisements from the previous test_multi calls.
	for (int i = 0; i < 4; ++i) {
		int ret = orb_unadvertise(_pfd[i]);

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

	//try to advertise and see whether we get the right instance
	int instance_test[4];
	struct orb_test t;

	for (int i = 0; i < 4; ++i) {
		_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);

		if (instance_test[i] != i) {
			return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]);
		}
	}

	for (int i = 0; i < 4; ++i) {
		orb_unadvertise(_pfd[i]);
	}

	return test_note("PASS unadvertise");
}
/**
 * Creates a fake traffic measurement with supplied parameters.
 *
 */
void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
			     float altitude_diff, float hor_velocity, float ver_velocity)
{
	double lat, lon;
	waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat,
					   &lon);
	float alt = get_global_position()->alt + altitude_diff;

	// float vel_n = get_global_position()->vel_n;
	// float vel_e = get_global_position()->vel_e;
	// float vel_d = get_global_position()->vel_d;

	transponder_report_s tr = {};
	tr.timestamp = hrt_absolute_time();
	tr.ICAO_address = 1234;
	tr.lat = lat; // Latitude, expressed as degrees
	tr.lon = lon; // Longitude, expressed as degrees
	tr.altitude_type = 0;
	tr.altitude = alt;
	tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians
	tr.hor_velocity	= hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
	tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up
	strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign));
	tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum
	tr.tslc = 2; // Time since last communication in seconds
	tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
		   transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
		   transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
		   transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
	tr.squawk = 6667;

	orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
	(void)orb_unadvertise(h);
}
Exemple #3
0
MavlinkULog::~MavlinkULog()
{
	if (_ulog_stream_ack_pub) {
		orb_unadvertise(_ulog_stream_ack_pub);
	}
	if (_ulog_stream_sub >= 0) {
		orb_unsubscribe(_ulog_stream_sub);
	}
}
MavlinkParametersManager::~MavlinkParametersManager()
{
	if (_uavcan_parameter_value_sub >= 0) {
		orb_unsubscribe(_uavcan_parameter_value_sub);
	}

	if (_uavcan_parameter_request_pub) {
		orb_unadvertise(_uavcan_parameter_request_pub);
	}
}
void
CameraTrigger::test()
{
	struct vehicle_command_s cmd = {};
	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
	cmd.param5 = 1.0f;

	orb_advert_t pub;
	pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
	(void)orb_unadvertise(pub);
}
Exemple #6
0
MPU9250::~MPU9250()
{
	/* make sure we are truly inactive */
	stop();
	_call_interval = 0;

	if (!_magnetometer_only) {
		orb_unadvertise(_accel_topic);
		orb_unadvertise(_gyro->_gyro_topic);
	}

	/* delete the accel subdriver */
	delete _accel;

	/* delete the gyro subdriver */
	delete _gyro;

	/* delete the magnetometer subdriver */
	delete _mag;

	/* free any existing reports */
	if (_accel_reports != nullptr) {
		delete _accel_reports;
	}

	if (_gyro_reports != nullptr) {
		delete _gyro_reports;
	}

	/* delete the perf counter */
	perf_free(_sample_perf);
	perf_free(_accel_reads);
	perf_free(_gyro_reads);
	perf_free(_bad_transfers);
	perf_free(_bad_registers);
	perf_free(_good_transfers);
	perf_free(_reset_retries);
	perf_free(_duplicates);
}
Exemple #7
0
OutputBase::~OutputBase()
{
	if (_vehicle_attitude_sub >= 0) {
		orb_unsubscribe(_vehicle_attitude_sub);
	}

	if (_vehicle_global_position_sub >= 0) {
		orb_unsubscribe(_vehicle_global_position_sub);
	}

	if (_mount_orientation_pub) {
		orb_unadvertise(_mount_orientation_pub);
	}
}
int uORBTest::UnitTest::pub_test_multi2_main()
{
	int data_next_idx = 0;
	const int num_instances = 3;
	orb_advert_t orb_pub[num_instances];
	struct orb_test_medium data_topic;

	for (int i = 0; i < num_instances; ++i) {
		orb_advert_t &pub = orb_pub[i];
		int idx = i;
//		PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time());
		pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT);

		if (idx != i) {
			_thread_should_exit = true;
			PX4_ERR("Got wrong instance! should be: %i, but is %i", i, idx);
			return -1;
		}
	}

	usleep(100 * 1000);

	int message_counter = 0, num_messages = 50 * num_instances;

	while (message_counter++ < num_messages) {
		usleep(2); //make sure the timestamps are different
		orb_advert_t &pub = orb_pub[data_next_idx];

		data_topic.time = hrt_absolute_time();
		data_topic.val = data_next_idx;

		orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic);
//		PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time);

		data_next_idx = (data_next_idx + 1) % num_instances;

		if (data_next_idx == 0) {
			usleep(50 * 1000);
		}
	}

	usleep(100 * 1000);
	_thread_should_exit = true;

	for (int i = 0; i < num_instances; ++i) {
		orb_unadvertise(orb_pub[i]);
	}

	return 0;
}
Exemple #9
0
MPU9250::~MPU9250()
{
	/* make sure we are truly inactive */
	stop();

	orb_unadvertise(_accel_topic);
	orb_unadvertise(_gyro->_gyro_topic);

	/* delete the gyro subdriver */
	delete _gyro;

	/* delete the magnetometer subdriver */
	delete _mag;

	/* free any existing reports */
	if (_accel_reports != nullptr) {
		delete _accel_reports;
	}

	if (_gyro_reports != nullptr) {
		delete _gyro_reports;
	}

	if (_accel_class_instance != -1) {
		unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
	}

	/* delete the perf counter */
	perf_free(_sample_perf);
	perf_free(_accel_reads);
	perf_free(_gyro_reads);
	perf_free(_bad_transfers);
	perf_free(_bad_registers);
	perf_free(_good_transfers);
	perf_free(_reset_retries);
	perf_free(_duplicates);
}
Exemple #10
0
void
CameraTrigger::test()
{
	struct vehicle_command_s cmd = {
		.timestamp = hrt_absolute_time(),
		.param5 = 1.0f,
		.param6 = 0.0f,
		.param1 = 0.0f,
		.param2 = 0.0f,
		.param3 = 0.0f,
		.param4 = 0.0f,
		.param7 = 0.0f,
		.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL
	};

	orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
	(void)orb_unadvertise(pub);
}
Exemple #11
0
SF1XX::~SF1XX()
{
	/* make sure we are truly inactive */
	stop();

	/* free any existing reports */
	if (_reports != nullptr) {
		delete _reports;
	}

	if (_distance_sensor_topic != nullptr) {
		orb_unadvertise(_distance_sensor_topic);
	}

	if (_class_instance != -1) {
		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
	}

	/* free perf counters */
	perf_free(_sample_perf);
	perf_free(_comms_errors);
}
Exemple #12
0
leddar_one::~leddar_one()
{
	stop();

	free((char *)_serial_port);

	if (_fd > -1) {
		::close(_fd);
	}

	if (_reports) {
		delete _reports;
	}

	if (_topic) {
		orb_unadvertise(_topic);
	}

	perf_free(_sample_perf);
	perf_free(_collect_timeout_perf);
	perf_free(_comm_error);
}
Exemple #13
0
LIS3MDL::~LIS3MDL()
{
	/* make sure we are truly inactive */
	stop();

	if (_mag_topic != nullptr) {
		orb_unadvertise(_mag_topic);
	}

	if (_reports != nullptr) {
		delete _reports;
	}

	if (_class_instance != -1) {
		unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
	}

	// free perf counters
	perf_free(_sample_perf);
	perf_free(_comms_errors);
	perf_free(_range_errors);
	perf_free(_conf_errors);
}
int uORBTest::UnitTest::pub_test_queue_main()
{
	struct orb_test_medium t;
	orb_advert_t ptopic;
	const unsigned int queue_size = 50;
	t.val = 0;

	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
		_thread_should_exit = true;
		return test_fail("advertise failed: %d", errno);
	}

	int message_counter = 0, num_messages = 20 * queue_size;
	++t.val;

	while (message_counter < num_messages) {

		//simulate burst
		int burst_counter = 0;

		while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned
			orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t);
			++t.val;
		}

		message_counter += burst_counter;
		usleep(20 * 1000); //give subscriber a chance to catch up
	}

	_num_messages_sent = t.val;
	usleep(100 * 1000);
	_thread_should_exit = true;
	orb_unadvertise(ptopic);

	return 0;
}
Exemple #15
0
OutputMavlink::~OutputMavlink()
{
	if (_vehicle_command_pub) {
		orb_unadvertise(_vehicle_command_pub);
	}
}
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");
}
void *multirotor_attitude_control_thread_main()
{
	/* welcome user */
	fprintf (stdout, "Multirotor attitude controller started\n");
	fflush(stdout);

	int i;
	bool_t updated, rates_sp_updated, reset_integral;
	float rates[3];

	/* store last control mode to detect mode switches */
	bool_t control_yaw_position = 1 /* true */;
	bool_t reset_yaw_sp = 1 /* true */;

	/* declare and safely initialize all structs */
	struct parameter_update_s update;
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_attitude_setpoint_s att_sp;
	memset(&att_sp, 0, sizeof(att_sp));
	struct offboard_control_setpoint_s offboard_sp;
	memset(&offboard_sp, 0, sizeof(offboard_sp));
	struct vehicle_control_flags_s control_flags;
	memset(&control_flags, 0, sizeof(control_flags));
	struct manual_control_setpoint_s manual;
	memset(&manual, 0, sizeof(manual));
	struct sensor_combined_s sensor;
	memset(&sensor, 0, sizeof(sensor));
	struct vehicle_rates_setpoint_s rates_sp;
	memset(&rates_sp, 0, sizeof(rates_sp));
	struct vehicle_status_s status;
	memset(&status, 0, sizeof(status));
	struct actuator_controls_s actuators;
	memset(&actuators, 0, sizeof(actuators));


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

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


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


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


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


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


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


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


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


	/* publish actuator controls */
	for (i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
		actuators.control[i] = 0.0f;
	}

	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	if (actuator_pub == -1)
	{
		fprintf (stderr, "Comunicator thread failed to advertise the actutor_controls topic\n");
		exit (-1);
	}
	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);

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

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



	while (!_shutdown_all_systems) {
		/* wait for a sensor update, check for exit condition every 500 ms */
		updated = orb_poll(ORB_ID(vehicle_attitude), vehicle_attitude_sub, 500000);

		/* timed out - periodic check for _shutdown_all_systems, etc. */
		if (!updated)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (updated < 0) {
			fprintf (stderr, "Attitude controller failed to poll vehicle attitude\n");
			continue;
		}

		/* only run controller if attitude changed */
		/* attitude */
		orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);


		/* parameters */
		updated = orb_check (ORB_ID(parameter_update), parameter_update_sub);
		if (updated) {
			orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
			/* update parameters */
		}

		/* control mode */
		updated = orb_check (ORB_ID(vehicle_control_flags), vehicle_control_flags_sub);
		if (updated) {
			orb_copy(ORB_ID(vehicle_control_flags), vehicle_control_flags_sub, &control_flags);
		}

		/* manual control setpoint */
		updated = orb_check (ORB_ID(manual_control_setpoint), manual_control_setpoint_sub);
		if (updated) {
			orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
		}

		/* attitude setpoint */
		updated = orb_check (ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub);
		if (updated) {
			orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
		}

		/* offboard control setpoint */
		updated = orb_check (ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub);
		if (updated) {
			orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
		}

		/* vehicle status */
		updated = orb_check (ORB_ID(vehicle_status), vehicle_status_sub);
		if (updated) {
			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
		}

		/* sensors */
		updated = orb_check (ORB_ID(sensor_combined), sensor_combined_sub);
		if (updated) {
			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
		}

		/* set flag to safe value */
		control_yaw_position = 1 /* true */;

		/* reset yaw setpoint if not armed */
		if (!control_flags.flag_armed) {
			reset_yaw_sp = 1 /* true */;
		}

		/* define which input is the dominating control input */
		if (control_flags.flag_control_offboard_enabled) {
			/* offboard inputs */
			if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
				rates_sp.roll = offboard_sp.p1;
				rates_sp.pitch = offboard_sp.p2;
				rates_sp.yaw = offboard_sp.p3;
				rates_sp.thrust = offboard_sp.p4;
				rates_sp.timestamp = get_absolute_time();
				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);

			} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
				att_sp.roll_body = offboard_sp.p1;
				att_sp.pitch_body = offboard_sp.p2;
				att_sp.yaw_body = offboard_sp.p3;
				att_sp.thrust = offboard_sp.p4;
				att_sp.timestamp = get_absolute_time();
				/* publish the result to the vehicle actuators */
				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
			}

			/* reset yaw setpoint after offboard control */
			reset_yaw_sp = 1 /* true */;

		} else if (control_flags.flag_control_manual_enabled) {
			/* manual input */
			if (control_flags.flag_control_attitude_enabled) {
				/* control attitude, update attitude setpoint depending on mode */
				if (att_sp.thrust < 0.1f) {
					/* no thrust, don't try to control yaw */
					rates_sp.yaw = 0.0f;
					control_yaw_position = 0 /* false */;

					if (status.condition_landed) {
						/* reset yaw setpoint if on ground */
						reset_yaw_sp = 1 /* true */;
					}

				} else {
					/* only move yaw setpoint if manual input is != 0 */
					if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
						/* control yaw rate */
						control_yaw_position = 0 /* false */;
						rates_sp.yaw = manual.yaw;
						reset_yaw_sp = 1 /* true */;	// has no effect on control, just for beautiful log

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

				if (!control_flags.flag_control_velocity_enabled) {
					/* update attitude setpoint if not in position control mode */
					att_sp.roll_body = manual.roll;
					att_sp.pitch_body = manual.pitch;

					if (!control_flags.flag_control_climb_rate_enabled) {
						/* pass throttle directly if not in altitude control mode */
						att_sp.thrust = manual.thrust;
					}
				}

				/* reset yaw setpint to current position if needed */
				if (reset_yaw_sp) {
					att_sp.yaw_body = att.yaw;
					reset_yaw_sp = 0 /* false */;
				}

				/*if (motor_test_mode) {
					printf("testmode");
					att_sp.roll_body = 0.0f;
					att_sp.pitch_body = 0.0f;
					att_sp.yaw_body = 0.0f;
					att_sp.thrust = 0.1f;
				}*/

				att_sp.timestamp = get_absolute_time();

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

			} else {
				/* manual rate inputs (ACRO), from RC control or joystick */
				if (control_flags.flag_control_rates_enabled) {
					rates_sp.roll = manual.roll;
					rates_sp.pitch = manual.pitch;
					rates_sp.yaw = manual.yaw;
					rates_sp.thrust = manual.thrust;
					rates_sp.timestamp = get_absolute_time();
				}

				/* reset yaw setpoint after ACRO */
				reset_yaw_sp = 1 /* true */;
			}

		} else {
			if (!control_flags.flag_control_auto_enabled) {
				/* no control, try to stay on place */
				if (!control_flags.flag_control_velocity_enabled) {
					/* no velocity control, reset attitude setpoint */
					att_sp.roll_body = 0.0f;
					att_sp.pitch_body = 0.0f;
					att_sp.timestamp = get_absolute_time();

					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
				}
			}

			/* reset yaw setpoint after non-manual control */
			reset_yaw_sp = 1 /* true */;
		}

		/* check if we should we reset integrals */
		reset_integral = !control_flags.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle

		/* run attitude controller if needed */
		if (control_flags.flag_control_attitude_enabled) {
			multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);

			orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
		}

		/* run rates controller if needed */
		if (control_flags.flag_control_rates_enabled) {
			/* get current rate setpoint */
			rates_sp_updated = orb_check (ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub);
			if (rates_sp_updated) {
				orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
			}

			/* apply controller */
			rates[0] = att.roll_rate;
			rates[1] = att.pitch_rate;
			rates[2] = att.yaw_rate;

			multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);

		} else {
			/* rates controller disabled, set actuators to zero for safety */
			actuators.control[0] = 0.0f;
			actuators.control[1] = 0.0f;
			actuators.control[2] = 0.0f;
			actuators.control[3] = 0.0f;
		}

		/* fill in manual control values */
		actuators.control[4] = manual.flaps;
		//actuators.control[5] = manual.aux1;
		//actuators.control[6] = manual.aux2;
		//actuators.control[7] = manual.aux3;

		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
	}

	fprintf (stdout, "INFO: Attitude controller exiting, disarming motors\n");
	fflush (stdout);

	/* kill all outputs */
	for (i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
		actuators.control[i] = 0.0f;

	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);

	/*
	 * do unsubscriptions
	 */
	orb_unsubscribe(ORB_ID(vehicle_attitude), vehicle_attitude_sub, pthread_self());
	orb_unsubscribe(ORB_ID(parameter_update), parameter_update_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, pthread_self());
	orb_unsubscribe(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_control_flags), vehicle_control_flags_sub, pthread_self());
	orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, pthread_self());
	orb_unsubscribe(ORB_ID(sensor_combined), sensor_combined_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, pthread_self());
	orb_unsubscribe(ORB_ID(vehicle_status), vehicle_status_sub, pthread_self());

	/*
	 * do unadvertises
	 */
	orb_unadvertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, pthread_self());
	orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self());
	orb_unadvertise(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, pthread_self());


	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 #19
0
void
GPS::task_main()
{
	/* open the serial port */
	_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);

	if (_serial_fd < 0) {
		PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);

		/* tell the dtor that we are exiting, set error code */
		_task = -1;
		px4_task_exit(1);
	}

#ifndef __PX4_QURT
	// TODO: this call is not supported on Snapdragon just yet.
	// However it seems to be nonblocking anyway and working.
	int flags = fcntl(_serial_fd, F_GETFL, 0);
	fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK);
#endif

	for (int i = 0; i < _orb_inject_data_fd_count; ++i) {
		_orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i);
	}

	initializeCommunicationDump();

	uint64_t last_rate_measurement = hrt_absolute_time();
	unsigned last_rate_count = 0;

	/* loop handling received serial bytes and also configuring in between */
	while (!_task_should_exit) {

		if (_fake_gps) {
			_report_gps_pos.timestamp = hrt_absolute_time();
			_report_gps_pos.lat = (int32_t)47.378301e7f;
			_report_gps_pos.lon = (int32_t)8.538777e7f;
			_report_gps_pos.alt = (int32_t)1200e3f;
			_report_gps_pos.s_variance_m_s = 10.0f;
			_report_gps_pos.c_variance_rad = 0.1f;
			_report_gps_pos.fix_type = 3;
			_report_gps_pos.eph = 0.9f;
			_report_gps_pos.epv = 1.8f;
			_report_gps_pos.vel_n_m_s = 0.0f;
			_report_gps_pos.vel_e_m_s = 0.0f;
			_report_gps_pos.vel_d_m_s = 0.0f;
			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
							_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
			_report_gps_pos.cog_rad = 0.0f;
			_report_gps_pos.vel_ned_valid = true;

			/* no time and satellite information simulated */


			publish();

			usleep(2e5);

		} else {

			if (_helper != nullptr) {
				delete(_helper);
				/* set to zero to ensure parser is not used while not instantiated */
				_helper = nullptr;
			}

			switch (_mode) {
			case GPS_DRIVER_MODE_UBX:
				_helper = new GPSDriverUBX(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
				break;

			case GPS_DRIVER_MODE_MTK:
				_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
				break;

			case GPS_DRIVER_MODE_ASHTECH:
				_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
				break;

			default:
				break;
			}


			/* the Ashtech driver lies about successful configuration and the
			 * MTK driver is not well tested, so we really only trust the UBX
			 * driver for an advance publication
			 */
			if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) {

				/* reset report */
				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));

				if (_mode == GPS_DRIVER_MODE_UBX) {
					/* Publish initial report that we have access to a GPS,
					 * but set all critical state fields to indicate we have
					 * no valid position lock
					 */

					/* reset the timestamp for data, because we have no data yet */
					_report_gps_pos.timestamp = 0;
					_report_gps_pos.timestamp_time_relative = 0;

					/* set a massive variance */
					_report_gps_pos.eph = 10000.0f;
					_report_gps_pos.epv = 10000.0f;
					_report_gps_pos.fix_type = 0;

					publish();

					/* GPS is obviously detected successfully, reset statistics */
					_helper->resetUpdateRates();
				}

				int helper_ret;

				while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {

					if (helper_ret & 1) {
						publish();

						last_rate_count++;
					}

					if (_p_report_sat_info && (helper_ret & 2)) {
						publishSatelliteInfo();
					}

					/* measure update rate every 5 seconds */
					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
						float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
						_rate = last_rate_count / dt;
						_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
						last_rate_measurement = hrt_absolute_time();
						last_rate_count = 0;
						_last_rate_rtcm_injection_count = 0;
						_helper->storeUpdateRates();
						_helper->resetUpdateRates();
					}

					if (!_healthy) {
						// Helpful for debugging, but too verbose for normal ops
//						const char *mode_str = "unknown";
//
//						switch (_mode) {
//						case GPS_DRIVER_MODE_UBX:
//							mode_str = "UBX";
//							break;
//
//						case GPS_DRIVER_MODE_MTK:
//							mode_str = "MTK";
//							break;
//
//						case GPS_DRIVER_MODE_ASHTECH:
//							mode_str = "ASHTECH";
//							break;
//
//						default:
//							break;
//						}
//
//						PX4_WARN("module found: %s", mode_str);
						_healthy = true;
					}

					//check for disarming
					if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) {
						bool updated;
						orb_check(_vehicle_status_sub, &updated);

						if (updated) {
							vehicle_status_s vehicle_status;
							orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status);
							bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
								     (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);

							if (armed) {
								_is_armed = true;

							} else if (_is_armed) {
								//disable communication dump when disarming
								close(_dump_from_gps_device_fd);
								_dump_from_gps_device_fd = -1;
								close(_dump_to_gps_device_fd);
								_dump_to_gps_device_fd = -1;
								_is_armed = false;
							}

						}
					}
				}

				if (_healthy) {
					PX4_WARN("GPS module lost");
					_healthy = false;
					_rate = 0.0f;
					_rate_rtcm_injection = 0.0f;
				}
			}

			/* select next mode */
			switch (_mode) {
			case GPS_DRIVER_MODE_UBX:
				_mode = GPS_DRIVER_MODE_MTK;
				break;

			case GPS_DRIVER_MODE_MTK:
				_mode = GPS_DRIVER_MODE_ASHTECH;
				break;

			case GPS_DRIVER_MODE_ASHTECH:
				_mode = GPS_DRIVER_MODE_UBX;
				break;

			default:
				break;
			}
		}

	}

	PX4_WARN("exiting");

	for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) {
		orb_unsubscribe(_orb_inject_data_fd[i]);
		_orb_inject_data_fd[i] = -1;
	}

	if (_dump_to_gps_device_fd != -1) {
		close(_dump_to_gps_device_fd);
		_dump_to_gps_device_fd = -1;
	}

	if (_dump_from_gps_device_fd != -1) {
		close(_dump_from_gps_device_fd);
		_dump_from_gps_device_fd = -1;
	}

	if (_vehicle_status_sub != -1) {
		orb_unsubscribe(_vehicle_status_sub);
		_vehicle_status_sub = -1;
	}

	::close(_serial_fd);

	orb_unadvertise(_report_gps_pos_pub);

	/* tell the dtor that we are exiting */
	_task = -1;
	px4_task_exit(0);
}
/**
 * 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;
}
Exemple #21
0
void
GPS::task_main()
{
	/* open the serial port */
	_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);

	if (_serial_fd < 0) {
		PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);

		/* tell the dtor that we are exiting, set error code */
		_task = -1;
		px4_task_exit(1);
	}

	_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));

	initializeCommunicationDump();

	uint64_t last_rate_measurement = hrt_absolute_time();
	unsigned last_rate_count = 0;

	/* loop handling received serial bytes and also configuring in between */
	while (!_task_should_exit) {

		if (_fake_gps) {
			_report_gps_pos.timestamp = hrt_absolute_time();
			_report_gps_pos.lat = (int32_t)47.378301e7f;
			_report_gps_pos.lon = (int32_t)8.538777e7f;
			_report_gps_pos.alt = (int32_t)1200e3f;
			_report_gps_pos.alt_ellipsoid = 10000;
			_report_gps_pos.s_variance_m_s = 0.5f;
			_report_gps_pos.c_variance_rad = 0.1f;
			_report_gps_pos.fix_type = 3;
			_report_gps_pos.eph = 0.8f;
			_report_gps_pos.epv = 1.2f;
			_report_gps_pos.hdop = 0.9f;
			_report_gps_pos.vdop = 0.9f;
			_report_gps_pos.vel_n_m_s = 0.0f;
			_report_gps_pos.vel_e_m_s = 0.0f;
			_report_gps_pos.vel_d_m_s = 0.0f;
			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
							_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
			_report_gps_pos.cog_rad = 0.0f;
			_report_gps_pos.vel_ned_valid = true;
			_report_gps_pos.satellites_used = 10;

			/* no time and satellite information simulated */


			publish();

			usleep(2e5);

		} else {

			if (_helper != nullptr) {
				delete(_helper);
				/* set to zero to ensure parser is not used while not instantiated */
				_helper = nullptr;
			}

			switch (_mode) {
			case GPS_DRIVER_MODE_NONE:
				_mode = GPS_DRIVER_MODE_UBX;

			//no break
			case GPS_DRIVER_MODE_UBX:
				_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
				break;

			case GPS_DRIVER_MODE_MTK:
				_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
				break;

			case GPS_DRIVER_MODE_ASHTECH:
				_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
				break;

			default:
				break;
			}


			/* the Ashtech driver lies about successful configuration and the
			 * MTK driver is not well tested, so we really only trust the UBX
			 * driver for an advance publication
			 */
			if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) {

				/* reset report */
				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));

				if (_mode == GPS_DRIVER_MODE_UBX) {
					/* Publish initial report that we have access to a GPS,
					 * but set all critical state fields to indicate we have
					 * no valid position lock
					 */

					/* reset the timestamp for data, because we have no data yet */
					_report_gps_pos.timestamp = 0;
					_report_gps_pos.timestamp_time_relative = 0;

					/* set a massive variance */
					_report_gps_pos.eph = 10000.0f;
					_report_gps_pos.epv = 10000.0f;
					_report_gps_pos.fix_type = 0;

					publish();

					/* GPS is obviously detected successfully, reset statistics */
					_helper->resetUpdateRates();
				}

				int helper_ret;

				while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {

					if (helper_ret & 1) {
						publish();

						last_rate_count++;
					}

					if (_p_report_sat_info && (helper_ret & 2)) {
						publishSatelliteInfo();
					}

					/* measure update rate every 5 seconds */
					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
						float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
						_rate = last_rate_count / dt;
						_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
						last_rate_measurement = hrt_absolute_time();
						last_rate_count = 0;
						_last_rate_rtcm_injection_count = 0;
						_helper->storeUpdateRates();
						_helper->resetUpdateRates();
					}

					if (!_healthy) {
						// Helpful for debugging, but too verbose for normal ops
//						const char *mode_str = "unknown";
//
//						switch (_mode) {
//						case GPS_DRIVER_MODE_UBX:
//							mode_str = "UBX";
//							break;
//
//						case GPS_DRIVER_MODE_MTK:
//							mode_str = "MTK";
//							break;
//
//						case GPS_DRIVER_MODE_ASHTECH:
//							mode_str = "ASHTECH";
//							break;
//
//						default:
//							break;
//						}
//
//						PX4_WARN("module found: %s", mode_str);
						_healthy = true;
					}
				}

				if (_healthy) {
					PX4_WARN("GPS module lost");
					_healthy = false;
					_rate = 0.0f;
					_rate_rtcm_injection = 0.0f;
				}
			}

			if (_mode_auto) {
				switch (_mode) {
				case GPS_DRIVER_MODE_UBX:
					_mode = GPS_DRIVER_MODE_MTK;
					break;

				case GPS_DRIVER_MODE_MTK:
					_mode = GPS_DRIVER_MODE_ASHTECH;
					break;

				case GPS_DRIVER_MODE_ASHTECH:
					_mode = GPS_DRIVER_MODE_UBX;
					break;

				default:
					break;
				}
			}

		}
	}

	PX4_INFO("exiting");

	orb_unsubscribe(_orb_inject_data_fd);

	if (_dump_communication_pub) {
		orb_unadvertise(_dump_communication_pub);
	}

	::close(_serial_fd);

	orb_unadvertise(_report_gps_pos_pub);

	/* tell the dtor that we are exiting */
	_task = -1;
	px4_task_exit(0);
}
Exemple #22
0
PublicationBase::~PublicationBase()
{
	orb_unadvertise(getHandle());
}
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) {
		PX4_INFO("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));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_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));
	_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();
	global_position_update();
	local_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[1] = {};

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

	bool global_pos_available_once = false;

	/* rate-limit global pos subscription to 20 Hz / 50 ms */
	orb_set_interval(_global_pos_sub, 49);

	while (!_task_should_exit) {

		/* wait for up to 1000ms 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("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_ERR("nav: poll error %d, %d", pret, errno);
			usleep(10000);
			continue;

		} else {

			if (fds[0].revents & POLLIN) {
				/* success, global pos is available */
				global_position_update();

				if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
					have_geofence_position_data = true;
				}

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

		/* local position updated */
		orb_check(_local_pos_sub, &updated);

		if (updated) {
			local_position_update();
		}

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

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

				if (home_position_valid()) {
					rep->current.yaw = cmd.param4;
					rep->previous.valid = true;

				} else {
					rep->current.yaw = get_local_position()->yaw;
					rep->previous.valid = false;
				}

				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->current.valid = true;
				rep->next.valid = false;

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

				/* find NAV_CMD_DO_LAND_START in the mission and
				 * use MAV_CMD_MISSION_START to start the mission there
				 */
				int land_start = _mission.find_offboard_land_start();

				if (land_start != -1) {
					vehicle_command_s vcmd = {};
					vcmd.target_system = get_vstatus()->system_id;
					vcmd.target_component = get_vstatus()->component_id;
					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
					vcmd.param1 = land_start;
					vcmd.param2 = 0;

					publish_vehicle_cmd(vcmd);

				} else {
					PX4_WARN("planned landing not available");
				}

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

				vehicle_command_s vcmd = {};
				vcmd.target_system = get_vstatus()->system_id;
				vcmd.target_component = get_vstatus()->component_id;
				vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;

				orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
				(void)orb_unadvertise(pub);

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

				if (get_mission_result()->valid &&
				    PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {

					_mission.set_current_offboard_mission_index(cmd.param1);
				}

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

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
				if (cmd.param2 > FLT_EPSILON) {
					// XXX not differentiating ground and airspeed yet
					set_cruising_speed(cmd.param2);

				} else {
					set_cruising_speed();

					/* if no speed target was given try to set throttle */
					if (cmd.param3 > FLT_EPSILON) {
						set_cruising_throttle(cmd.param3 / 100);

					} else {
						set_cruising_throttle();
					}
				}
			}
		}

		/* 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.timestamp = hrt_absolute_time();
			_geofence_result.geofence_action = _geofence.getGeofenceAction();

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

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

				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}

			publish_geofence_result();
		}

		/* Do stuff according to navigation state set by commander */
		switch (_vstatus.nav_state) {
		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
			_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;
			_navigation_mode = &_rcLoss;
			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:
			_pos_sp_triplet_published_invalid_once = false;
			_navigation_mode = &_dataLinkLoss;
			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;

		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:
		case vehicle_status_s::NAVIGATION_STATE_STAB:
		default:
			_pos_sp_triplet_published_invalid_once = false;
			_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) {
			_pos_sp_triplet.timestamp = hrt_absolute_time();
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

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

		perf_end(_loop_perf);
	}

	PX4_INFO("exiting");

	_navigator_task = -1;
}
Exemple #24
0
PX4Magnetometer::~PX4Magnetometer()
{
	if (_topic != nullptr) {
		orb_unadvertise(_topic);
	}
}
void *multirotor_position_control_thread_main()
{
	/* welcome user */
	fprintf (stdout, "Multirotor position controller started\n");
	fflush(stdout);

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

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

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

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

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


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


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

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


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


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


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


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


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


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

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

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

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



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

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

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


	while (!_shutdown_all_systems) {

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

			/* update multirotor_position_control_parameters */
			multirotor_position_control_params_update();

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

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

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

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

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

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

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

		absolute_time t = get_absolute_time();
		float dt;

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

		} else {
			dt = 0.0f;
		}

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

		was_armed = control_flags.flag_armed;

		t_prev = t;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

					reset_man_sp_z = 1 /* true */;

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

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

					reset_auto_sp_z = 0 /* false */;
				}

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

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

					reset_auto_sp_xy = 0 /* false */;
				}
			}

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

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

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

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

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

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

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

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

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

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

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

						if (reset_int_z_manual) {
							i = manual.thrust;

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

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

						thrust_pid_set_integral(&z_vel_pid, -i);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

	return 0;
}