diff options
Diffstat (limited to 'apps/fixedwing_control/fixedwing_control.c')
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 938 |
1 files changed, 938 insertions, 0 deletions
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c new file mode 100644 index 000000000..d9d39f14d --- /dev/null +++ b/apps/fixedwing_control/fixedwing_control.c @@ -0,0 +1,938 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Ivan Ovinnikov <oivan@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file fixedwing_control.c + * Implementation of a fixed wing attitude and position controller. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <math.h> +#include <termios.h> +#include <time.h> +#include <arch/board/up_hrt.h> +#include <arch/board/board.h> +#include <arch/board/drv_pwm_servo.h> +#include <nuttx/spi.h> +#include "../mix_and_link/mix_and_link.h" +#include "fixedwing_control.h" + +__EXPORT int fixedwing_control_main(int argc, char *argv[]); + +#define PID_DT 0.01f +#define PID_SCALER 0.1f +#define PID_DERIVMODE_CALC 0 +#define HIL_MODE 32 +#define RAD2DEG ((1.0/180.0)*M_PI) +#define AUTO -1000 +#define MANUAL 3000 +#define SERVO_MIN 1000 +#define SERVO_MAX 2000 + +pthread_t control_thread; +pthread_t nav_thread; +pthread_t servo_thread; + +/** + * Servo channels function enumerator used for + * the servo writing part + */ + +enum SERVO_CHANNELS_FUNCTION { + + AIL_1 = 0, + AIL_2 = 1, + MOT = 2, + ACT_1 = 3, + ACT_2 = 4, + ACT_3 = 5, + ACT_4 = 6, + ACT_5 = 7 +}; + +/** + * The plane_data structure. + * + * The plane data structure is the local storage of all the flight information of the aircraft + */ +typedef struct { + double lat; + double lon; + float alt; + float vx; + float vy; + float vz; + float yaw; + float hdg; + float pitch; + float roll; + float yawspeed; + float pitchspeed; + float rollspeed; + float rollb; /* body frame angles */ + float pitchb; + float yawb; + float p; + float q; + float r; /* body angular rates */ + + /* PID parameters*/ + + float Kp_att; + float Ki_att; + float Kd_att; + float Kp_pos; + float Ki_pos; + float Kd_pos; + float intmax_att; + float intmax_pos; + + /* Next waypoint*/ + + float wp_x; + float wp_y; + float wp_z; + + /* Setpoints */ + + float airspeed; + float groundspeed; + float roll_setpoint; + float pitch_setpoint; + float throttle_setpoint; + + /* Navigation mode*/ + int mode; + +} plane_data_t; + +/** + * The control_outputs structure. + * + * The control outputs structure contains the control outputs + * of the aircraft + */ +typedef struct { + float roll_ailerons; + float pitch_elevator; + float yaw_rudder; + float throttle; + // set the aux values to 0 per default + float aux1; + float aux2; + float aux3; + float aux4; + uint8_t mode; // HIL_ENABLED: 32 + uint8_t nav_mode; +} control_outputs_t; + +/** + * Generic PID algorithm with PD scaling + */ +static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax); + +/* + * Output calculations + */ + +static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); +static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z); +static void calc_bodyframe_angles(float roll, float pitch, float yaw); +static float calc_bearing(void); +static float calc_roll_ail(void); +static float calc_pitch_elev(void); +static float calc_yaw_rudder(float hdg); +static float calc_throttle(void); +static float calc_gnd_speed(void); +static void get_parameters(void); +static float calc_roll_setpoint(void); +static float calc_pitch_setpoint(void); +static float calc_throttle_setpoint(void); +static float calc_wp_distance(void); +static void set_plane_mode(void); + +/* + * The control, navigation and servo loop threads + */ + +static void *control_loop(void *arg); +static void *nav_loop(void *arg); +static void *servo_loop(void *arg); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +plane_data_t plane_data; +control_outputs_t control_outputs; +float scaler = 1; //M_PI; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/** + * + * Calculates the PID control output given an error. Incorporates PD scaling and low-pass filter for the derivative component. + * + * @param error the input error + * @param error_deriv the derivative of the input error + * @param dt time constant + * @param scaler PD scaler + * @param K_p P gain + * @param K_i I gain + * @param K_d D gain + * @param intmax Integration limit + * + * @return the PID control output + */ + +static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax) +{ + float Kp = K_p; + float Ki = K_i; + float Kd = K_d; + float delta_time = dt; + float lerror; + float imax = intmax; + float integrator; + float derivative; + float lderiv; + int fCut = 20; /* anything above 20 Hz is considered noise - low pass filter for the derivative */ + float output = 0; + + output += error * Kp; + + if ((fabs(Kd) > 0) && (dt > 0)) { + + if (PID_DERIVMODE_CALC) { + derivative = (error - lerror) / delta_time; + + /* + * discrete low pass filter, cuts out the + * high frequency noise that can drive the controller crazy + */ + float RC = 1 / (2 * M_PI * fCut); + derivative = lderiv + + (delta_time / (RC + delta_time)) * (derivative - lderiv); + + /* update state */ + lerror = error; + lderiv = derivative; + + } else { + derivative = error_deriv; + } + + /* add in derivative component */ + output += Kd * derivative; + } + + //printf("PID derivative %i\n", (int)(1000*derivative)); + + /* scale the P and D components with the PD scaler */ + output *= scale; + + /* Compute integral component if time has elapsed */ + if ((fabs(Ki) > 0) && (dt > 0)) { + integrator += (error * Ki) * scaler * delta_time; + + if (integrator < -imax) { + integrator = -imax; + + } else if (integrator > imax) { + integrator = imax; + } + + output += integrator; + } + + //printf("PID Integrator %i\n", (int)(1000*integrator)); + + return output; +} + +/** + * Load parameters from global storage. + * + * Fetches the current parameters from the global parameter storage and writes them + * to the plane_data structure + */ + +static void get_parameters() +{ + plane_data.Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P]; + plane_data.Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I]; + plane_data.Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D]; + plane_data.Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P]; + plane_data.Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I]; + plane_data.Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D]; + plane_data.intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]; + plane_data.intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]; + plane_data.airspeed = global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED]; + plane_data.wp_x = global_data_parameter_storage->pm.param_values[PARAM_WPLON]; + plane_data.wp_y = global_data_parameter_storage->pm.param_values[PARAM_WPLAT]; + plane_data.wp_z = global_data_parameter_storage->pm.param_values[PARAM_WPALT]; + plane_data.mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE]; +} + +/** + * Calculates the body angular rates. + * + * Calculates the rates of the plane using inertia matrix and + * writes them to the plane_data structure + * + * @param roll + * @param pitch + * @param yaw + * @param rollspeed + * @param pitchspeed + * @param yawspeed + * + */ +static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ + plane_data.p = rollspeed - sinf(pitch) * yawspeed; + plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cos(pitch) * yawspeed; + plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cos(pitch) * yawspeed; +} + +/** + * + * Calculates the attitude angles in the body reference frame. + * + * Writes them to the plane data structure + * + * @param roll + * @param pitch + * @param yaw + */ + +static void calc_bodyframe_angles(float roll, float pitch, float yaw) +{ + plane_data.rollb = cosf(yaw) * cosf(pitch) * roll + + (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch + + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * yaw; + plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll + + (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch + + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw; + plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw; +} + +/** + * calc_rotation_matrix + * + * Calculates the rotation matrix + * + * @param roll + * @param pitch + * @param yaw + * @param x + * @param y + * @param z + * + */ + +static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z) +{ + plane_data.rollb = cosf(yaw) * cosf(pitch) * x + + (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y + + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * z; + plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x + + (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y + + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z; + plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z; +} + +/** + * calc_bearing + * + * Calculates the bearing error of the plane compared to the waypoints + * + * @return bearing Bearing error + * + */ + +static float calc_bearing() +{ + float bearing = 90 + atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)) * RAD2DEG; + + if (bearing < 0) + bearing += 360; + + return bearing; +} + +/** + * calc_roll_ail + * + * Calculates the roll ailerons control output + * + * @return Roll ailerons control output (-1,1) + */ + +static float calc_roll_ail() +{ + float ret = pid((plane_data.roll_setpoint - plane_data.roll) / scaler, plane_data.rollspeed, PID_DT, PID_SCALER, + plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); + + if (ret < -1) + return -1; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_pitch_elev + * + * Calculates the pitch elevators control output + * + * @return Pitch elevators control output (-1,1) + */ + +static float calc_pitch_elev() +{ + float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER, + plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); + + if (ret < -1) + return -1; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_yaw_rudder + * + * Calculates the yaw rudder control output (only if yaw rudder exists on the model) + * + * @return Yaw rudder control output (-1,1) + * + */ + +static float calc_yaw_rudder(float hdg) +{ + float ret = pid((plane_data.yaw / RAD2DEG - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER, + plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); + + if (ret < -1) + return -1; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_throttle + * + * Calculates the throttle control output + * + * @return Throttle control output (0,1) + */ + +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; +} + +/** + * calc_gnd_speed + * + * Calculates the ground speed using the x and y components + * + * Input: none (operation on global data) + * + * Output: Ground speed of the plane + * + */ + +static float calc_gnd_speed() +{ + float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy); + return gnd_speed; +} + +/** + * calc_wp_distance + * + * Calculates the distance to the next waypoint + * + * @return the distance to the next waypoint + * + */ + +static float calc_wp_distance() +{ + return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) + + (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x)); +} + +/** + * calc_roll_setpoint + * + * Calculates the offset angle for the roll plane, + * saturates at +- 35 deg. + * + * @return setpoint on which attitude control should stabilize while changing heading + * + */ + +static float calc_roll_setpoint() +{ + float setpoint = 0; + + if (plane_data.mode == TAKEOFF) { + setpoint = 0; + + } else { + setpoint = calc_bearing() - plane_data.yaw; + + if (setpoint < -35) + return -35; + + if (setpoint > 35) + return 35; + } + + return setpoint; +} + +/** + * calc_pitch_setpoint + * + * Calculates the offset angle for the pitch plane + * saturates at +- 35 deg. + * + * @return setpoint on which attitude control should stabilize while changing altitude + * + */ + +static float calc_pitch_setpoint() +{ + float setpoint = 0; + + if (plane_data.mode == TAKEOFF) { + setpoint = 35; + + } else { + setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()) * RAD2DEG; + + if (setpoint < -35) + return -35; + + if (setpoint > 35) + return 35; + } + + return setpoint; +} + +/** + * calc_throttle_setpoint + * + * Calculates the throttle setpoint for different flight modes + * + * @return throttle output setpoint + * + */ + +static float calc_throttle_setpoint() +{ + float setpoint = 0; + + // if TAKEOFF full throttle + if (plane_data.mode == TAKEOFF) { + setpoint = 60; + } + + // if CRUISE - parameter value + if (plane_data.mode == CRUISE) { + setpoint = plane_data.airspeed; + } + + // if LAND no throttle + if (plane_data.mode == LAND) { + setpoint = 0; + } + + return setpoint; +} + +/** + * set_plane_mode + * + * Sets the plane mode + * (TAKEOFF, CRUISE, LOITER or LAND) + * + */ + +static void set_plane_mode() +{ + if (plane_data.alt < 10) { + plane_data.mode = TAKEOFF; + + } else { + plane_data.mode = CRUISE; + // TODO: if reached waypoint and no further waypoint exists, go to LOITER mode + } + + // Debug override - don't need TAKEOFF mode for now + plane_data.mode = CRUISE; +} + +/* + * fixedwing_control_main + * + * @param argc number of arguments + * @param argv argument array + * + * @return 0 + * + */ + +int fixedwing_control_main(int argc, char *argv[]) +{ + /* print text */ + printf("Fixedwing control started\n"); + usleep(100000); + + /* default values for arguments */ + char *fixedwing_uart_name = "/dev/ttyS1"; + char *commandline_usage = "\tusage: fixedwing_control -d fixedwing-devicename\n"; + + /* read arguments */ + int 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); + return 0; + } + } + + /* 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)); + + /* Control constants */ + control_outputs.mode = HIL_MODE; + control_outputs.nav_mode = 0; + + /* Servo setup */ + + int fd; + servo_position_t data[PWM_SERVO_MAX_CHANNELS]; + + fd = open("/dev/pwm_servo", O_RDWR); + + if (fd < 0) { + printf("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_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; + 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; + + /* 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(); + + /* Attitude control part */ + +//#define MUTE +#ifndef MUTE + /******************************** DEBUG OUTPUT ************************************************************/ + + 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)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * 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)); + + /************************************************************************************************************/ + +#endif + + /* + * Computation section + * + * The function calls to compute the required control values happen + * in this section. + */ + + /* 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(); + + if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode + + 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) { + control.attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons); + control.attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator); + control.attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle); + //control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); + } + + control.counter++; + control.timestamp = hrt_absolute_time(); + } + + /* Navigation part */ + + // Get GPS Waypoint + + // if(global_data_wait(&global_data_position_setpoint->access_conf) == 0) + // { + // plane_data.wp_x = global_data_position_setpoint->x; + // plane_data.wp_y = global_data_position_setpoint->y; + // plane_data.wp_z = global_data_position_setpoint->z; + // } + // global_data_unlock(&global_data_position_setpoint->access_conf); + + 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(); + plane_data.pitch_setpoint = calc_pitch_setpoint(); + plane_data.throttle_setpoint = calc_throttle_setpoint(); + + } 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); + + // 10 Hz loop + usleep(100000); + + } else { + control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale; + control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale; + control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale; + // since we don't have a yaw rudder + //control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale; + + 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] = control.attitude_control_output[ROLL]; + buffer_rc[PITCH] = control.attitude_control_output[PITCH]; + buffer_rc[THROTTLE] = 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)) { + printf("failed writing servo outputs\n"); + } + + /* 20Hz loop*/ + usleep(50000); + } + + return 0; +} |