Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: move auto to control_auto.pde and fix yaw

This commit is contained in:
Randy Mackay 2014-01-23 14:14:58 +09:00 committed by Andrew Tridgell
parent 8e40cbdd7f
commit 1b714defcc
2 changed files with 224 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

206
ArduCopter/control_auto.pde Normal file
View File

@ -0,0 +1,206 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_auto.pde - init and run calls for auto, guided, land and RTL flight modes
*/
// auto_init - initialise auto controller
static bool auto_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialise auto_yaw_mode
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
cliSerial->printf_P(PSTR("\nYM:%d\n"),(int)auto_yaw_mode);
// clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
init_commands();
return true;
}else{
return false;
}
}
// auto_run - runs the auto controller
// should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
static void auto_run()
{
float target_yaw_rate = 0;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || !inertial_nav.position_ok()) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.set_throttle_out(0, false);
return;
}
// process pilot's yaw input
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
if (target_yaw_rate != 0) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// run waypoint controller
wp_nav.update_wpnav();
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading());
}
// body-frame rate controller is run directly from 100hz loop
// refetch angle targets for reporting
const Vector3f angle_target = attitude_control.angle_ef_targets();
control_roll = angle_target.x;
control_pitch = angle_target.y;
control_yaw = angle_target.z;
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t get_default_auto_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
}else{
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
break;
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
void set_auto_yaw_mode(uint8_t yaw_mode)
{
// return immediately if no change
if (auto_yaw_mode == yaw_mode) {
return;
}
auto_yaw_mode = yaw_mode;
// perform initialisation
switch (auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
// original_wp_bearing will be set by do_nav_wp or other nav command initialisatoin functions so no init required
break;
case AUTO_YAW_LOOK_AT_LOCATION:
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
yaw_look_at_heading = ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
yaw_look_ahead_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float get_auto_heading(void)
{
switch(auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_LOCATION:
// point towards a location held in yaw_look_at_WP
return get_look_at_yaw();
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return yaw_look_at_heading;
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw();
break;
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
control_yaw = initial_armed_bearing;
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
return original_wp_bearing;
break;
}
}
// guided_init - initialise guided controller
static bool guided_init(bool ignore_checks)
{
return true;
}
// guided_run - runs the guided controller
// should be called at 100hz or more
static void guided_run()
{
}
// land_init - initialise land controller
static bool land_init(bool ignore_checks)
{
return true;
}
// land_run - runs the land controller
// should be called at 100hz or more
static void land_run()
{
verify_land();
}
// rtl_init - initialise rtl controller
static bool rtl_init(bool ignore_checks)
{
return true;
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
static void rtl_run()
{
verify_RTL();
}

94
ArduCopter/control_stabilize.pde
View File

@ -30,6 +30,8 @@ static void acro_run()
attitude_control.rate_stab_bf_to_rate_bf_roll();
attitude_control.rate_stab_bf_to_rate_bf_pitch();
attitude_control.rate_stab_bf_to_rate_bf_yaw();
// pilot controlled yaw using rate controller
//get_yaw_rate_stabilized_bf(pilot_yaw);
// call get_acro_level_rates() here?
@ -182,46 +184,14 @@ static void althold_run()
control_yaw = angle_target.z;
}
// auto_init - initialise auto controller
static bool auto_init(bool ignore_checks)
{
return true;
}
// auto_run - runs the auto controller
// should be called at 100hz or more
static void auto_run()
{
Vector3f angle_target;
// run way point controller
// copy latest output from nav controller to stabilize controller
angle_target.x = wp_nav.get_roll();
angle_target.y = wp_nav.get_pitch();
// To-Do: handle pilot input for yaw and different methods to update yaw (ROI, face next wp)
angle_target.z = control_yaw;
// To-Do: shorten below by moving these often used steps into a single function in the AC_AttitudeControl lib
// set earth-frame angular targets
attitude_control.angle_ef_targets(angle_target);
// convert earth-frame angle targets to earth-frame rate targets
attitude_control.angle_to_rate_ef_roll();
attitude_control.angle_to_rate_ef_pitch();
attitude_control.angle_to_rate_ef_yaw();
// convert earth-frame rates to body-frame rates
attitude_control.rate_ef_targets_to_bf();
// body-frame rate controller is run directly from 100hz loop
}
// circle_init - initialise circle controller
static bool circle_init(bool ignore_checks)
{
// set yaw to point to center of circle
// yaw_look_at_WP = circle_center;
// initialise bearing to current heading
//yaw_look_at_WP_bearing = ahrs.yaw_sensor;
//yaw_initialised = true;
return true;
}
@ -229,6 +199,12 @@ static bool circle_init(bool ignore_checks)
// should be called at 100hz or more
static void circle_run()
{
// if we are landed reset yaw target to current heading
//if (ap.land_complete) {
// control_yaw = ahrs.yaw_sensor;
//}
// points toward the center of the circle or does a panorama
//get_circle_yaw();
}
// loiter_init - initialise loiter controller
@ -236,7 +212,6 @@ static bool loiter_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
// set target to current position
// To-Do: supply zero velocity below?
wp_nav.init_loiter_target();
return true;
}else{
@ -314,44 +289,6 @@ static void loiter_run()
control_yaw = angle_target.z;
}
// guided_init - initialise guided controller
static bool guided_init(bool ignore_checks)
{
return true;
}
// guided_run - runs the guided controller
// should be called at 100hz or more
static void guided_run()
{
}
// land_init - initialise land controller
static bool land_init(bool ignore_checks)
{
return true;
}
// land_run - runs the land controller
// should be called at 100hz or more
static void land_run()
{
verify_land();
}
// rtl_init - initialise rtl controller
static bool rtl_init(bool ignore_checks)
{
return true;
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
static void rtl_run()
{
verify_RTL();
}
// ofloiter_init - initialise ofloiter controller
static bool ofloiter_init(bool ignore_checks)
{
@ -374,6 +311,11 @@ static bool drift_init(bool ignore_checks)
// should be called at 100hz or more
static void drift_run()
{
// if we have landed reset yaw target to current heading
//if (ap.land_complete) {
// control_yaw = ahrs.yaw_sensor;
//}
//get_yaw_drift();
}
// sport_init - initialise sport controller