static float calc_throttle(plane_data_t * plane_data, pid_s_t *pid_s)
{
float ret = pid(plane_data->throttle_setpoint - calc_gnd_speed(plane_data), 0, PID_DT, PID_SCALER,
pid_s->Kp_pos, pid_s->Ki_pos, pid_s->Kd_pos, pid_s->intmax_pos, pid_s->lerror, pid_s->lderiv);
if (ret < 0.2f)
return 0.2f;
if (ret > 1.0f)
return 1.0f;
return ret;
}
static float calc_throttle()
{
float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER,
plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
if (ret < 0.2)
return 0.2;
if (ret > 1)
return 1;
return ret;
}
int fixedwing_control_main(int argc, char *argv[])
{
/* default values for arguments */
char *fixedwing_uart_name = "/dev/ttyS1";
char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
fixedwing_uart_name = argv[i + 1];
} else {
printf(commandline_usage, argv[0]);
return 0;
}
}
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing control] started\n");
/* Set up to publish fixed wing control messages */
struct fixedwing_control_s control;
int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
/* Subscribe to global position, attitude and rc */
struct vehicle_global_position_s global_pos;
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_attitude_s att;
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct rc_channels_s rc;
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
struct vehicle_global_position_setpoint_s global_setpoint;
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* Mainloop setup */
unsigned int loopcounter = 0;
unsigned int failcounter = 0;
/* Control constants */
control_outputs.mode = HIL_MODE;
control_outputs.nav_mode = 0;
/* Servo setup */
int fd;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
fd = open("/dev/pwm_servo", O_RDWR);
if (fd < 0) {
printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
}
ioctl(fd, PWM_SERVO_ARM, 0);
int16_t buffer_rc[3];
int16_t buffer_servo[3];
mixer_data_t mixer_buffer;
mixer_buffer.input = buffer_rc;
mixer_buffer.output = buffer_servo;
mixer_conf_t mixers[3];
mixers[0].source = PITCH;
mixers[0].nr_actuators = 2;
mixers[0].dest[0] = AIL_1;
mixers[0].dest[1] = AIL_2;
mixers[0].dual_rate[0] = 1;
mixers[0].dual_rate[1] = 1;
mixers[1].source = ROLL;
mixers[1].nr_actuators = 2;
mixers[1].dest[0] = AIL_1;
mixers[1].dest[1] = AIL_2;
mixers[1].dual_rate[0] = 1;
mixers[1].dual_rate[1] = -1;
mixers[2].source = THROTTLE;
mixers[2].nr_actuators = 1;
mixers[2].dest[0] = MOT;
mixers[2].dual_rate[0] = 1;
/*
* Main control, navigation and servo routine
*/
while(1) {
/*
* DATA Handling
* Fetch current flight data
*/
/* get position, attitude and rc inputs */
// XXX add error checking
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* scaling factors are defined by the data from the APM Planner
* TODO: ifdef for other parameters (HIL/Real world switch)
*/
/* position values*/
plane_data.lat = global_pos.lat; /// 10000000.0;
plane_data.lon = global_pos.lon; /// 10000000.0;
plane_data.alt = global_pos.alt; /// 1000.0f;
plane_data.vx = global_pos.vx / 100.0f;
plane_data.vy = global_pos.vy / 100.0f;
plane_data.vz = global_pos.vz / 100.0f;
/* attitude values*/
plane_data.roll = att.roll;
plane_data.pitch = att.pitch;
plane_data.yaw = att.yaw;
plane_data.rollspeed = att.rollspeed;
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
/* parameter values */
get_parameters(&plane_data, &pid_s);
/* Attitude control part */
if (verbose && loopcounter % 20 == 0) {
/******************************** DEBUG OUTPUT ************************************************************/
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)pid_s.Kp_att, (int)pid_s.Ki_att,
(int)pid_s.Kd_att, (int)pid_s.intmax_att, (int)pid_s.Kp_pos, (int)pid_s.Ki_pos,
(int)pid_s.Kd_pos, (int)pid_s.intmax_pos, (int)plane_data.airspeed,
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z,
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPALT],
global_setpoint.lat,
global_setpoint.lon,
(int)global_setpoint.altitude);
printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*plane_data.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(&plane_data)));
printf("Current Position: \n %i \n %i \n %i \n", (int)plane_data.lat, (int)plane_data.lon, (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)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
(int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
(int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator),
(int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle));
/************************************************************************************************************/
}
/*
* Computation section
*
* The function calls to compute the required control values happen
* in this section.
*/
/* Set plane mode */
set_plane_mode(&plane_data);
/* Calculate the output values */
control_outputs.roll_ailerons = calc_roll_ail(&plane_data, &pid_s);
control_outputs.pitch_elevator = calc_pitch_elev(&plane_data, &pid_s);
control_outputs.yaw_rudder = calc_yaw_rudder(&plane_data, &pid_s);
control_outputs.throttle = calc_throttle(&plane_data, &pid_s);
/*
* Calculate rotation matrices
*/
calc_rotation_matrix(&rmatrix_att, plane_data.roll, plane_data.pitch, plane_data.yaw);
calc_rotation_matrix(&rmatrix_c, control_outputs.roll_ailerons, control_outputs.pitch_elevator, control_outputs.yaw_rudder);
multiply_matrices(&rmatrix_att, &rmatrix_c, &rmatrix_b);
calc_angles_from_rotation_matrix(&rmatrix_b, plane_data.rollb, plane_data.pitchb, plane_data.yawb);
// TODO: fix RC input
//if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
/*
* TAKEOFF hack for HIL
*/
if (plane_data.mode == TAKEOFF) {
control.attitude_control_output[ROLL] = 0;
control.attitude_control_output[PITCH] = 5000;
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) {
// TODO: substitute output with the body angles (rollb, pitchb)
control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
control.attitude_control_output[THROTTLE] = control_outputs.throttle;
//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
control.counter++;
control.timestamp = hrt_absolute_time();
//}
/* Navigation part */
/*
* TODO: APM Planner Waypoint communication
*/
// Get GPS Waypoint
// plane_data.wp_x = global_setpoint.lon;
// plane_data.wp_y = global_setpoint.lat;
// plane_data.wp_z = global_setpoint.altitude;
/*
* TODO: fix RC input
*/
//if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode
// AUTO/HYBRID switch
//if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) {
plane_data.roll_setpoint = calc_roll_setpoint(35.0f, &plane_data);
plane_data.pitch_setpoint = calc_pitch_setpoint(35.0f, &plane_data);
plane_data.throttle_setpoint = calc_throttle_setpoint(&plane_data);
// } else {
// plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200;
// plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
// plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
// }
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
// } else {
// control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000;
// control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000;
// control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000;
// since we don't have a yaw rudder
//control.attitude_control_output[YAW] = rc.chan[rc.function[YAW]].scale/10000;
control.counter++;
control.timestamp = hrt_absolute_time();
//}
/* publish the control data */
orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
/* Servo part */
buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);
//mix_and_link(mixers, 3, 2, &mixer_buffer);
// Scaling and saturation of servo outputs happens here
data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];
if (data[AIL_1] > SERVO_MAX)
data[AIL_1] = SERVO_MAX;
if (data[AIL_1] < SERVO_MIN)
data[AIL_1] = SERVO_MIN;
data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];
if (data[AIL_2] > SERVO_MAX)
data[AIL_2] = SERVO_MAX;
if (data[AIL_2] < SERVO_MIN)
data[AIL_2] = SERVO_MIN;
data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];
if (data[MOT] > SERVO_MAX)
data[MOT] = SERVO_MAX;
if (data[MOT] < SERVO_MIN)
data[MOT] = SERVO_MIN;
int result = write(fd, &data, sizeof(data));
if (result != sizeof(data)) {
if (failcounter < 10 || failcounter % 20 == 0) {
printf("[fixedwing_control] failed writing servo outputs\n");
}
failcounter++;
}
loopcounter++;
/* 20Hz loop*/
usleep(50000);
}
return 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;
}