zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1026 Commits
14 Branches
0 Tags
306 MiB
 
 
 
 
 
 
Tree: f2ab85756c
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'f2ab85756c'
${ noResults }
px4_autopilot/apps/fixedwing_control/fixedwing_control.c

887 lines
23 KiB
Raw Blame History

/****************************************************************************
*
* Copyright (C) 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 <drivers/drv_pwm_output.h>
#include <nuttx/spi.h>
#include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/param/param.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 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(plane_data_t *plane_data);
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);
/****************************************************************************
* 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 ((fabsf(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.0f / (2.0f * M_PI_F * 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 ((fabsf(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;
}
PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f);
PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f);
PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f);
PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f);
PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f);
PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f);
/**
* Load parameters from global storage.
*
* @param plane_data Fixed wing data structure
*
* Fetches the current parameters from the global parameter storage and writes them
* to the plane_data structure
*/
static void get_parameters(plane_data_t * pdata)
{
static bool initialized = false;
static param_t att_p;
static param_t att_i;
static param_t att_d;
static param_t att_awu;
static param_t att_lim;
static param_t pos_p;
static param_t pos_i;
static param_t pos_d;
static param_t pos_awu;
static param_t pos_lim;
if (!initialized) {
att_p = param_find("FW_ATT_P");
att_i = param_find("FW_ATT_I");
att_d = param_find("FW_ATT_D");
att_awu = param_find("FW_ATT_AWU");
att_lim = param_find("FW_ATT_LIM");
pos_p = param_find("FW_POS_P");
pos_i = param_find("FW_POS_I");
pos_d = param_find("FW_POS_D");
pos_awu = param_find("FW_POS_AWU");
pos_lim = param_find("FW_POS_LIM");
initialized = true;
}
param_get(att_p, &(pdata->Kp_att));
param_get(att_i, &(pdata->Ki_att));
param_get(att_d, &(pdata->Kd_att));
param_get(pos_p, &(pdata->Kp_pos));
param_get(pos_i, &(pdata->Ki_pos));
param_get(pos_d, &(pdata->Kd_pos));
param_get(att_awu, &(pdata->intmax_att));
param_get(pos_awu, &(pdata->intmax_pos));
pdata->airspeed = 10;
pdata->wp_x = 48;
pdata->wp_y = 8;
pdata->wp_z = 100;
pdata->mode = 1;
}
/**
* 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) * cosf(pitch) * yawspeed;
plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(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 = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
if (bearing < 0.0f) {
bearing += 2*M_PI_F;
}
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), 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), 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 - abs(hdg)), 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.2f)
return 0.2f;
if (ret > 1.0f)
return 1.0f;
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.0f/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
if (setpoint > (35/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
}
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.0f;
if (plane_data.mode == TAKEOFF) {
setpoint = ((35/180.0f)*M_PI_F);
} else {
setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
if (setpoint < (-35.0f/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
if (setpoint > (35/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
}
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.0f;
}
// if CRUISE - parameter value
if (plane_data.mode == CRUISE) {
setpoint = plane_data.airspeed;
}
// if LAND no throttle
if (plane_data.mode == LAND) {
setpoint = 0.0f;
}
return setpoint;
}
/**
* set_plane_mode
*
* Sets the plane mode
* (TAKEOFF, CRUISE, LOITER or LAND)
*
*/
static void set_plane_mode()
{
if (plane_data.alt < 10.0f) {
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[])
{
/* 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");
/* output structs */
struct actuator_controls_s actuators;
struct actuator_armed_s armed;
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
armed.armed = false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
/* 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_global_position_setpoint_s global_setpoint;
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_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 */
/*
* 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);
/* get a local copy of system state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
/* 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 */
if (loopcounter % 20 == 0) {
get_parameters(&plane_data);
}
/* Attitude control part */
if (verbose && loopcounter % 20 == 0) {
/******************************** 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)(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*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)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F),
(int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F);
// 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: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n",
att_sp.roll_body, att_sp.pitch_body,
att_sp.yaw_body, att_sp.thrust);
/************************************************************************************************************/
}
/* Set plane mode */
set_plane_mode();
/* Calculate the P,Q,R body rates of the aircraft */
//calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
// plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
/* Calculate the body frame angles of the aircraft */
//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
// if (manual.override_mode_switch < XXX) {
/* Navigation part */
// Get Waypoint
// XXX FIXME
//else if (manual.override_mode_switch > MANUAL) { // AUTO mode
/* calculate setpoint based on current position and waypoint */
att_sp.roll_body = calc_roll_setpoint();
att_sp.pitch_body = calc_pitch_setpoint();
att_sp.thrust = calc_throttle_setpoint();
/* calculate the control outputs based on roll / pitch / yaw setpoints */
actuators.control[0] = calc_roll_ail();
actuators.control[1] = calc_pitch_elev();
actuators.control[2] = calc_yaw_rudder(att.yaw);
actuators.control[3] = calc_throttle();
/* publish attitude setpoint (for MAVLink) */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
/* publish actuator setpoints (for mixer) */
/* arming state depends on commander arming state */
armed.armed = state.flag_system_armed;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
loopcounter++;
/* 20Hz loop*/
usleep(50000);
}
return 0;
}