Exemplo n.º 1
0
static int
mc_thread_main(int argc, char *argv[])
{
	/* structures */
	struct vehicle_status_s state;
	struct vehicle_attitude_s att;
	struct rc_channels_s rc;
	struct actuator_controls_s actuators;
	struct actuator_armed_s armed;

	/* subscribe to attitude, motor setpoints and system state */
	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
	int rc_sub = orb_subscribe(ORB_ID(rc_channels));

	/* rate-limit the attitude subscription to 200Hz to pace our loop */
	orb_set_interval(att_sub, 5);
	struct pollfd fds = { .fd = att_sub, .events = POLLIN };

	/* publish actuator controls */
	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
		actuators.control[i] = 0.0f;
	int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
	armed.armed = true;
	int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);

	/* register the perf counter */
	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_control");

	/* welcome user */
	printf("[multirotor_control] starting\n");

	while (!thread_should_exit) {

		/* wait for a sensor update */
		poll(&fds, 1, -1);

		perf_begin(mc_loop_perf);

		/* get a local copy of the vehicle state */
		orb_copy(ORB_ID(vehicle_status), state_sub, &state);

		/* get a local copy of rc inputs */
		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);

		/* get a local copy of attitude */
		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);

		/* run the attitude controller */
		multirotor_control_attitude(&rc, &att, &state, &actuators);

		/* publish the result */
		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);

		perf_end(mc_loop_perf);
	}

	printf("[multirotor_control] stopping\r\n");

	/* kill all outputs */
	armed.armed = false;
	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
		actuators.control[i] = 0.0f;
	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);


	close(att_sub);
	close(state_sub);
	close(rc_sub);
	close(actuator_pub);
	close(armed_pub);

	perf_print_counter(mc_loop_perf);
	perf_free(mc_loop_perf);

	fflush(stdout);
	exit(0);
}
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;
}