fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 10:03:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_att_control/fixedwing_att_control_main.c
+46 -12
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;
apps/fixedwing_att_control/fixedwing_att_control_rate.c
+5 -3
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++;