예제 #1
0
/**This gets all current values and writes them to min or max
 */
uint8_t get_value_s(void)
{
	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
		uint8_t i;

		for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
			//see if the value is bigger or smaller than 1500 (roughly neutral)
			//and write to the appropriate array
			if (global_data_rc_channels->chan[i].raw != 0 &&
			    global_data_rc_channels->chan[i].raw < 1500) {
				min_values_servo[i] = global_data_rc_channels->chan[i].raw;

			} else if (global_data_rc_channels->chan[i].raw != 0 &&
				   global_data_rc_channels->chan[i].raw > 1500) {
				max_values_servo[i] = global_data_rc_channels->chan[i].raw;

			} else {
				max_values_servo[i] = 0;
				min_values_servo[i] = 0;
			}
		}

		global_data_unlock(&global_data_rc_channels->access_conf);
		return 0;

	} else
		return -1;
}
예제 #2
0
static void *heartbeatloop(void * arg)
{

	/* initialize waypoint manager */
	mavlink_wpm_init(&wpm);
	/* initialize parameter storage */
	mavlink_pm_reset_params(&pm);

	int lowspeed_counter = 0;
	int result_sys_status_trylock;

	while(1) {

		// sleep
		usleep(50000);

		// 1 Hz
		if (lowspeed_counter == 10)
		{
			/* Send heartbeat */
			mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_GENERIC,MAV_MODE_PREFLIGHT,custom_mode,MAV_STATE_UNINIT);


			/* Send status */
			result_sys_status_trylock = global_data_trylock(&global_data_sys_status.access_conf);
			if(0 == result_sys_status_trylock) mavlink_msg_sys_status_send(chan, global_data_sys_status.onboard_control_sensors_present, global_data_sys_status.onboard_control_sensors_enabled, global_data_sys_status.onboard_control_sensors_health, global_data_sys_status.load, global_data_sys_status.voltage_battery, global_data_sys_status.current_battery, global_data_sys_status.battery_remaining, global_data_sys_status.drop_rate_comm, global_data_sys_status.errors_comm, global_data_sys_status.errors_count1, global_data_sys_status.errors_count2, global_data_sys_status.errors_count3, global_data_sys_status.errors_count4);
			global_data_unlock(&global_data_sys_status.access_conf);

			if (!(mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
				// System is not armed, blink at 1 Hz
				led_toggle(LED_BLUE);
			}

			lowspeed_counter = 0;
		}
		lowspeed_counter++;

		// Send one parameter
		mavlink_pm_queued_send();

		if (mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED) {
			// System is armed, blink at 10 Hz
			led_toggle(LED_BLUE);
		}
		usleep(50000);
	}

}
예제 #3
0
/**This sets the middle values
 */
uint8_t set_mid_s(void)
{
	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
		uint8_t i;

		for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
			if (i == global_data_rc_channels->function[ROLL] ||
			    i == global_data_rc_channels->function[YAW] ||
			    i == global_data_rc_channels->function[PITCH]) {

				mid_values_servo[i] = global_data_rc_channels->chan[i].raw;

			} else {
				mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2;
			}

		}

		global_data_unlock(&global_data_rc_channels->access_conf);
		return 0;

	} else
		return -1;
}
예제 #4
0
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
 const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
 const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{
	static PID_t distance_controller;

	static int read_ret;
	static global_data_position_t position_estimated;

	static uint16_t counter;

	static bool initialized;
	static uint16_t pm_counter;

	static float lat_next;
	static float lon_next;

	static float pitch_current;

	static float thrust_total;


	if (initialized == false) {

		pid_init(&distance_controller,
			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
			 PID_MODE_DERIVATIV_CALC, 150);//150

//		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
//		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];

		thrust_total = 0.0f;

		/* Position initialization */
		/* Wait for new position estimate */
		do {
			read_ret = read_lock_position(&position_estimated);
		} while (read_ret != 0);

		lat_next = position_estimated.lat;
		lon_next = position_estimated.lon;

		/* attitude initialization */
		global_data_lock(&global_data_attitude->access_conf);
		pitch_current = global_data_attitude->pitch;
		global_data_unlock(&global_data_attitude->access_conf);

		initialized = true;
	}

	/* load new parameters with 10Hz */
	if (counter % 50 == 0) {
		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
			/* check whether new parameters are available */
			if (global_data_parameter_storage->counter > pm_counter) {
				pid_set_parameters(&distance_controller,
						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);

//
//				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
//				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];

				pm_counter = global_data_parameter_storage->counter;
				printf("Position controller changed pid parameters\n");
			}
		}

		global_data_unlock(&global_data_parameter_storage->access_conf);
	}


	/* Wait for new position estimate */
	do {
		read_ret = read_lock_position(&position_estimated);
	} while (read_ret != 0);

	/* Get next waypoint */ //TODO: add local copy

	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
		lat_next = global_data_position_setpoint->x;
		lon_next = global_data_position_setpoint->y;
		global_data_unlock(&global_data_position_setpoint->access_conf);
	}

	/* Get distance to waypoint */
	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
//	if(counter % 5 == 0)
//		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);

	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);

	if (counter % 5 == 0)
		printf("bearing: %.4f\n", bearing);

	/* Calculate speed in direction of bearing (needed for controller) */
	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
//	if(counter % 5 == 0)
//		printf("speed_norm: %.4f\n", speed_norm);
	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller

	/* Control Thrust in bearing direction  */
	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else

//	if(counter % 5 == 0)
//		printf("horizontal thrust: %.4f\n", horizontal_thrust);

	/* Get total thrust (from remote for now) */
	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
		global_data_unlock(&global_data_rc_channels->access_conf);
	}

	const float max_gas = 500.0f;
	thrust_total *= max_gas / 20000.0f; //TODO: check this
	thrust_total += max_gas / 2.0f;


	if (horizontal_thrust > thrust_total) {
		horizontal_thrust = thrust_total;

	} else if (horizontal_thrust < -thrust_total) {
		horizontal_thrust = -thrust_total;
	}



	//TODO: maybe we want to add a speed controller later...

	/* Calclulate thrust in east and north direction */
	float thrust_north = cosf(bearing) * horizontal_thrust;
	float thrust_east = sinf(bearing) * horizontal_thrust;

	if (counter % 10 == 0) {
		printf("thrust north: %.4f\n", thrust_north);
		printf("thrust east: %.4f\n", thrust_east);
		fflush(stdout);
	}

	/* Get current attitude */
	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
		pitch_current = global_data_attitude->pitch;
		global_data_unlock(&global_data_attitude->access_conf);
	}

	/* Get desired pitch & roll */
	float pitch_desired = 0.0f;
	float roll_desired = 0.0f;

	if (thrust_total != 0) {
		float pitch_fraction = -thrust_north / thrust_total;
		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);

		if (roll_fraction < -1) {
			roll_fraction = -1;

		} else if (roll_fraction > 1) {
			roll_fraction = 1;
		}

//		if(counter % 5 == 0)
//		{
//			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
//			fflush(stdout);
//		}

		pitch_desired = asinf(pitch_fraction);
		roll_desired = asinf(roll_fraction);
	}

	att_sp.roll = roll_desired;
	att_sp.pitch = pitch_desired;

	counter++;
}
예제 #5
0
static void *control_loop(void *arg)
{
	// Set thread name
	prctl(1, "fixedwing_control attitude", getpid());

	control_outputs.mode = HIL_MODE + 16;
	control_outputs.nav_mode = 0;

	while (1) {

		/************************************
		 * Read global data structures here
		 ************************************/

		// Get parameters
		get_parameters();

#define MUTE
#ifndef MUTE
		///// DEBUG OUTPUT ////////

//		printf("Throttle: \tChannel %i\t Value %i\n", global_data_rc_channels->function[THROTTLE], global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale);
//		printf("Throttle_correct: \tChannel %i\t Value %i\n", THROTTLE, global_data_rc_channels->chan[THROTTLE].scale);
//		printf("Override: Channel %i\t Value %i\n", global_data_rc_channels->function[OVERRIDE], global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale);
//		printf("Override_correct: \tChannel %i\t Value %i\n", OVERRIDE, global_data_rc_channels->chan[OVERRIDE].scale);

		printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
		       (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
		       (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
		       (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);

//		printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
//		printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
//		printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());

		printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));

		printf("Current Altitude: %i\n\n", (int)plane_data.alt);

		printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
		       (int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
		       (int)(1000 * plane_data.rollspeed), (int)(1000 * plane_data.pitchspeed), (int)(1000 * plane_data.yawspeed));

		printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
		       (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));

		printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
		       (int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
		       (int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));

//		printf("\nGlobal attitude outputs \n, %i\n%i\n%i\n", global_data_fixedwing_control->attitude_control_output[ROLL],
//			global_data_fixedwing_control->attitude_control_output[PITCH], global_data_fixedwing_control->attitude_control_output[THROTTLE]);

		///////////////////////////

#endif

		// position values

		if (global_data_trylock(&global_pos.access_conf) == 0) {
			plane_data.lat = global_pos.lat / 10000000;
			plane_data.lon = global_pos.lon / 10000000;
			plane_data.alt = global_pos.alt / 1000;
			plane_data.vx = global_pos.vx / 100;
			plane_data.vy = global_pos.vy / 100;
			plane_data.vz = global_pos.vz / 100;
		}

		global_data_unlock(&global_pos.access_conf);

		// attitude values

		if (global_data_trylock(&att.access_conf) == 0) {
			plane_data.roll = att.roll;
			plane_data.pitch = att.pitch;
			plane_data.yaw = att.yaw;
			plane_data.hdg = att.yaw - 180;
			plane_data.rollspeed = att.rollspeed;
			plane_data.pitchspeed = att.pitchspeed;
			plane_data.yawspeed = att.yawspeed;
		}

		global_data_unlock(&att.access_conf);

		// Set plane mode
		set_plane_mode();

		// Calculate the P,Q,R body rates of the aircraft
		calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG,
					plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);

		// Calculate the body frame angles of the aircraft
		//calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG);

		// Calculate the output values
		control_outputs.roll_ailerons = calc_roll_ail();
		control_outputs.pitch_elevator = calc_pitch_elev();
		//control_outputs.yaw_rudder = calc_yaw_rudder();
		control_outputs.throttle = calc_throttle();


		/*******************************************
		 * Send data to global data structure
		 ******************************************/

		if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode

			if (plane_data.mode == TAKEOFF) {
				global_data_fixedwing_control->attitude_control_output[ROLL] = 0;
				global_data_fixedwing_control->attitude_control_output[PITCH] = 5000;
				global_data_fixedwing_control->attitude_control_output[THROTTLE] = 10000;
				//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
			}

			if (plane_data.mode == CRUISE) {
				global_data_fixedwing_control->attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons);
				global_data_fixedwing_control->attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator);
				global_data_fixedwing_control->attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle);
				//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
			}

			global_data_fixedwing_control->counter++;
			global_data_fixedwing_control->timestamp = hrt_absolute_time();

#error Either publish here or above, not at two locations
			orb_publish()
		}

		// 20 Hz loop
		usleep(100000);
	}

	return NULL;
}