fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mode switching for all platforms, additional fixed wing modes

Browse Source
This commit is contained in:
Lorenz Meier 2012-11-25 13:55:28 +01:00
parent dc72d467d4
commit faa672f8bb
5 changed files with 215 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
apps/commander/commander.c
View File

@ -1496,14 +1496,20 @@ int commander_thread_main(int argc, char *argv[])
}
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
if (sp_man.aux1_cam_pan_flaps < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
//update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
// XXX hack
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
}
}
/* handle the case where RC signal was regained */

58
apps/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -62,8 +62,8 @@
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <fixedwing_att_control_rate.h>
#include <fixedwing_att_control_att.h>
@ -126,7 +126,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
}
/* welcome user */
printf("[fixedwing att_control] started\n");
printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
@ -153,6 +153,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
// debug_output.key[0] = '1';
@ -185,19 +186,51 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* Control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
/* Attitude Control */
fixedwing_att_control_attitude(&att_sp,
&att,
&rates_sp);
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, &rates_sp);
/* publish rate setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* Attitude Rate Control */
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX
actuators.control[3] = 0.7f;
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = 0.5f;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
att_sp.timestamp = hrt_absolute_time();
}
fixedwing_att_control_attitude(&att_sp, &att, &rates_sp);
/* publish rate setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
// debug_output.value = rates_sp.pitch;
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
@ -224,6 +257,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
close(att_sub);
close(actuator_pub);
close(rates_pub);
fflush(stdout);
exit(0);
@ -268,7 +302,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
deamon_task = task_spawn("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
2048,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;

8
apps/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,6 +34,8 @@
/**
* @file fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#include <fixedwing_att_control_rate.h>
@ -171,8 +173,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer...
actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
actuators->control[2] = 0.0f;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now
counter++;

246
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
View File

@ -57,9 +57,11 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
/*
@ -162,16 +164,16 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
// bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
// for (int i = 1; i < argc; i++) {
// if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
// verbose = true;
// }
// }
/* welcome user */
printf("[fixedwing att_control] started\n");
printf("[fixedwing pos control] started\n");
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos;
@ -184,6 +186,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err));
struct parameter_update_s param_update;
memset(&param_update, 0, sizeof(param_update));
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
@ -193,129 +197,151 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
attitude_setpoint.thrust = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
struct pollfd fds[2] = {
{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }
};
bool global_sp_updated_set_once = false;
float psi_track = 0.0f;
int counter = 0;
struct fw_pos_control_params p;
struct fw_pos_control_param_handles h;
PID_t heading_controller;
PID_t altitude_controller;
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
while(!thread_should_exit)
{
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
int ret = poll(fds, 2, 500);
static int counter = 0;
static bool initialized = false;
if (ret < 0) {
/* poll error, count it in perf */
perf_count(fw_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
static struct fw_pos_control_params p;
static struct fw_pos_control_param_handles h;
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
PID_t heading_controller;
PID_t altitude_controller;
if(!initialized)
{
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
}
/* Check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* Load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if(pos_updated)
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
if(global_sp_updated) {
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* Control */
/* Simple Horizontal Control */
if(global_sp_updated_set_once)
{
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
if(!(distance_res != OK || xtrack_err.past_end)) {
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
}
else {
if (counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
/* check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
}
if (global_sp_updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* simple horizontal control, execute if line between wps is known */
if(global_sp_updated_set_once)
{
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
if(!(distance_res != OK || xtrack_err.past_end)) {
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
}
else {
if (counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
}
// XXX SIMPLE ALTITUDE, BUT NO SPEED CONTROL
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
// XXX need speed control
attitude_setpoint.thrust = 0.7f;
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
/* measure in what intervals the controller runs */
perf_count(fw_interval_perf);
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
}
/* Very simple Altitude Control */
if(global_sp_updated_set_once && pos_updated)
{
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
/*Publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
counter++;
}
printf("[fixedwing_pos_control] exiting.\n");
@ -367,7 +393,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
deamon_task = task_spawn("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
2048,
fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;

17
apps/sensors/sensors.cpp
View File

@ -973,7 +973,7 @@ Sensors::ppm_poll()
}
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
@ -990,6 +990,21 @@ Sensors::ppm_poll()
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
/* aux inputs */
manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled;
if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f;
if (manual_control.aux1_cam_pan_flaps > 1.0f) manual_control.aux1_cam_pan_flaps = 1.0f;
/* aux inputs */
manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled;
if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f;
if (manual_control.aux2_cam_tilt > 1.0f) manual_control.aux2_cam_tilt = 1.0f;
/* aux inputs */
manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled;
if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f;
if (manual_control.aux3_cam_zoom > 1.0f) manual_control.aux3_cam_zoom = 1.0f;
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
}