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

Copter: replace yaw_mode with auto_yaw_mode

This commit is contained in:
Randy Mackay 2014-01-23 14:16:06 +09:00 committed by Andrew Tridgell
parent 1b714defcc
commit 65f7bf92f4
5 changed files with 56 additions and 299 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

241
ArduCopter/ArduCopter.pde
View File

@ -657,8 +657,6 @@ static int32_t baro_alt;
// Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes
// Each Flight mode is a unique combination of these modes
//
// The current desired control scheme for Yaw
static uint8_t yaw_mode = STABILIZE_YAW;
// The current desired control scheme for roll and pitch / navigation
static uint8_t roll_pitch_mode = STABILIZE_RP;
// The current desired control scheme for altitude hold
@ -710,16 +708,20 @@ static uint32_t throttle_integrator;
////////////////////////////////////////////////////////////////////////////////
// Navigation Yaw control
////////////////////////////////////////////////////////////////////////////////
// auto flight mode's yaw mode
static uint8_t auto_yaw_mode = AUTO_YAW_LOOK_AT_NEXT_WP;
// The Commanded Yaw from the autopilot.
static int32_t control_yaw;
// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION
static Vector3f yaw_look_at_WP;
// bearing from current location to the yaw_look_at_WP
static int32_t yaw_look_at_WP_bearing;
static float yaw_look_at_WP_bearing;
// yaw used for YAW_LOOK_AT_HEADING yaw_mode
static int32_t yaw_look_at_heading;
// Deg/s we should turn
static int16_t yaw_look_at_heading_slew;
// heading when in yaw_look_ahead_bearing
static float yaw_look_ahead_bearing;
@ -1359,239 +1361,6 @@ static void update_GPS(void)
failsafe_gps_check();
}
// set_yaw_mode - update yaw mode and initialise any variables required
bool set_yaw_mode(uint8_t new_yaw_mode)
{
// boolean to ensure proper initialisation of throttle modes
bool yaw_initialised = false;
// return immediately if no change
if( new_yaw_mode == yaw_mode ) {
return true;
}
switch( new_yaw_mode ) {
case YAW_HOLD:
yaw_initialised = true;
break;
case YAW_ACRO:
yaw_initialised = true;
acro_yaw_rate = 0;
break;
case YAW_LOOK_AT_NEXT_WP:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_LOOK_AT_LOCATION:
if( ap.home_is_set ) {
// update bearing - assumes yaw_look_at_WP has been intialised before set_yaw_mode was called
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
yaw_initialised = true;
}
break;
case YAW_CIRCLE:
if( ap.home_is_set ) {
// 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;
}
break;
case YAW_LOOK_AT_HEADING:
yaw_initialised = true;
break;
case YAW_LOOK_AT_HOME:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_LOOK_AHEAD:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_DRIFT:
yaw_initialised = true;
break;
case YAW_RESETTOARMEDYAW:
control_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one
yaw_initialised = true;
break;
}
// if initialisation has been successful update the yaw mode
if( yaw_initialised ) {
yaw_mode = new_yaw_mode;
}
// return success or failure
return yaw_initialised;
}
// update_yaw_mode - run high level yaw controllers
// 100hz update rate
void update_yaw_mode(void)
{
int16_t pilot_yaw = g.rc_4.control_in;
// do not process pilot's yaw input during radio failsafe
if (failsafe.radio) {
pilot_yaw = 0;
}
switch(yaw_mode) {
case YAW_HOLD:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}
// heading hold at heading held in control_yaw but allow input from pilot
get_yaw_rate_stabilized_ef(pilot_yaw);
break;
case YAW_ACRO:
// pilot controlled yaw using rate controller
get_yaw_rate_stabilized_bf(pilot_yaw);
break;
case YAW_LOOK_AT_NEXT_WP:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}else{
// point towards next waypoint (no pilot input accepted)
// we don't use wp_bearing because we don't want the copter to turn too much during flight
control_yaw = get_yaw_slew(control_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE);
}
get_stabilize_yaw(control_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if (pilot_yaw != 0) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_LOOK_AT_LOCATION:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}
// point towards a location held in yaw_look_at_WP
get_look_at_yaw();
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if (pilot_yaw != 0) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_CIRCLE:
// 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();
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if (pilot_yaw != 0) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_LOOK_AT_HOME:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}else{
// keep heading always pointing at home with no pilot input allowed
control_yaw = get_yaw_slew(control_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
}
get_stabilize_yaw(control_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if (pilot_yaw != 0) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_LOOK_AT_HEADING:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}else{
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
control_yaw = get_yaw_slew(control_yaw, yaw_look_at_heading, yaw_look_at_heading_slew);
}
get_stabilize_yaw(control_yaw);
break;
case YAW_LOOK_AHEAD:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}
// Commanded Yaw to automatically look ahead.
get_look_ahead_yaw(pilot_yaw);
break;
case YAW_DRIFT:
// if we have landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}
get_yaw_drift();
break;
case YAW_RESETTOARMEDYAW:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
control_yaw = ahrs.yaw_sensor;
}else{
// changes yaw to be same as when quad was armed
control_yaw = get_yaw_slew(control_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE);
}
get_stabilize_yaw(control_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if (pilot_yaw != 0) {
set_yaw_mode(YAW_HOLD);
}
break;
}
}
// get yaw mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t get_wp_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
return YAW_LOOK_AT_NEXT_WP;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if( rtl ) {
return YAW_HOLD;
}else{
return YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return YAW_LOOK_AHEAD;
break;
default:
return YAW_HOLD;
break;
}
}
// set_roll_pitch_mode - update roll/pitch mode and initialise any variables as required
bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
{

18
ArduCopter/Attitude.pde
View File

@ -588,32 +588,26 @@ static void get_circle_yaw()
// get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller
// should be called at 100hz
static void get_look_at_yaw()
static float get_look_at_yaw()
{
static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz
look_at_yaw_counter++;
if( look_at_yaw_counter >= 10 ) {
if (look_at_yaw_counter >= 10) {
look_at_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
}
// slew yaw and call stabilize controller
control_yaw = get_yaw_slew(control_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(control_yaw);
return yaw_look_at_WP_bearing;
}
static void get_look_ahead_yaw(int16_t pilot_yaw)
static float get_look_ahead_yaw()
{
// Commanded Yaw to automatically look ahead.
if (g_gps->fix && g_gps->ground_speed_cm > YAW_LOOK_AHEAD_MIN_SPEED) {
control_yaw = get_yaw_slew(control_yaw, g_gps->ground_course_cd, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(wrap_360_cd(control_yaw + pilot_yaw)); // Allow pilot to "skid" around corners up to 45 degrees
}else{
control_yaw += pilot_yaw * g.acro_yaw_p * G_Dt;
control_yaw = wrap_360_cd(control_yaw);
get_stabilize_yaw(control_yaw);
yaw_look_ahead_bearing = g_gps->ground_course_cd;
}
return yaw_look_ahead_bearing;
}
/*************************************************************

38
ArduCopter/commands_logic.pde
View File

@ -238,7 +238,7 @@ static void do_takeoff()
set_roll_pitch_mode(AUTO_RP);
// set yaw mode
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// set throttle mode to AUTO although we should already be in this mode
set_throttle_mode(AUTO_THR);
@ -286,7 +286,7 @@ static void do_nav_wp()
}
// set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter
set_yaw_mode(get_wp_yaw_mode(false));
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
// do_land - initiate landing procedure
@ -304,7 +304,7 @@ static void do_land(const struct Location *cmd)
set_roll_pitch_mode(AUTO_RP);
// set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter
set_yaw_mode(get_wp_yaw_mode(false));
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// set throttle mode
set_throttle_mode(AUTO_THR);
@ -340,7 +340,7 @@ static void do_land(const struct Location *cmd)
}
// hold yaw while landing
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// set throttle mode to land
set_throttle_mode(THROTTLE_LAND);
@ -361,7 +361,7 @@ static void do_loiter_unlimited()
set_throttle_mode(AUTO_THR);
// hold yaw
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// get current position
Vector3f curr_pos = inertial_nav.get_position();
@ -407,7 +407,7 @@ static void do_circle()
}
// set yaw to point to center of circle
set_yaw_mode(CIRCLE_YAW);
set_auto_yaw_mode(CIRCLE_YAW);
// set angle travelled so far to zero
circle_angle_total = 0;
@ -429,7 +429,7 @@ static void do_loiter_time()
set_throttle_mode(AUTO_THR);
// hold yaw
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// get current position
Vector3f curr_pos = inertial_nav.get_position();
@ -491,7 +491,7 @@ static bool verify_land()
set_roll_pitch_mode(ROLL_PITCH_LOITER);
// give pilot control of yaw
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// set throttle mode to land
set_throttle_mode(THROTTLE_LAND);
@ -587,7 +587,7 @@ static bool verify_RTL()
// if we are below rtl alt do initial climb
if( current_loc.alt < get_RTL_alt() ) {
// first stage of RTL is the initial climb so just hold current yaw
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// use projection of safe stopping point based on current location and velocity
Vector3f origin, dest;
@ -601,7 +601,7 @@ static bool verify_RTL()
rtl_state = RTL_STATE_INITIAL_CLIMB;
}else{
// point nose towards home (maybe)
set_yaw_mode(get_wp_yaw_mode(true));
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
// Set wp navigation target to above home
wp_nav.set_wp_destination(Vector3f(0,0,get_RTL_alt()));
@ -628,7 +628,7 @@ static bool verify_RTL()
original_wp_bearing = wp_bearing;
// point nose towards home (maybe)
set_yaw_mode(get_wp_yaw_mode(true));
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
// advance to next rtl state
rtl_state = RTL_STATE_RETURNING_HOME;
@ -644,10 +644,10 @@ static bool verify_RTL()
rtl_loiter_start_time = millis();
// give pilot back control of yaw
if(get_wp_yaw_mode(true) != YAW_HOLD) {
set_yaw_mode(YAW_RESETTOARMEDYAW); // yaw back to initial yaw on take off
if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW); // yaw back to initial yaw on take off
} else {
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// advance to next rtl state
@ -669,7 +669,7 @@ static bool verify_RTL()
wp_nav.set_loiter_target(Vector3f(0,0,0));
// hold yaw while landing
set_yaw_mode(YAW_HOLD);
set_auto_yaw_mode(AUTO_YAW_HOLD);
// set throttle mode to land
set_throttle_mode(THROTTLE_LAND);
@ -770,7 +770,7 @@ static void do_yaw()
}
// set yaw mode
set_yaw_mode(YAW_LOOK_AT_HEADING);
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
// TO-DO: restore support for clockwise / counter clockwise rotation held in command_cond_queue.p1
// command_cond_queue.p1; // 0 = undefined, 1 = clockwise, -1 = counterclockwise
@ -845,7 +845,7 @@ static void do_guided(const struct Location *cmd)
wp_bearing = wp_nav.get_wp_bearing_to_destination();
// point nose at next waypoint if it is more than 10m away
if (yaw_mode == YAW_LOOK_AT_NEXT_WP) {
if (auto_yaw_mode == AUTO_YAW_LOOK_AT_NEXT_WP) {
// get distance to new location
wp_distance = wp_nav.get_wp_distance_to_destination();
// set original_wp_bearing to point at next waypoint
@ -911,7 +911,7 @@ static void do_roi()
// check if mount type requires us to rotate the quad
if( camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll ) {
yaw_look_at_WP = pv_location_to_vector(command_cond_queue);
set_yaw_mode(YAW_LOOK_AT_LOCATION);
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION);
}
// send the command to the camera mount
camera_mount.set_roi_cmd(&command_cond_queue);
@ -925,7 +925,7 @@ static void do_roi()
#else
// if we have no camera mount aim the quad at the location
yaw_look_at_WP = pv_location_to_vector(command_cond_queue);
set_yaw_mode(YAW_LOOK_AT_LOCATION);
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION);
#endif
}

14
ArduCopter/defines.h
View File

@ -15,16 +15,16 @@
// Flight modes
// ------------
#define YAW_HOLD 0 // heading hold at heading in control_yaw but allow input from pilot
#define AUTO_YAW_HOLD 0 // pilot controls the heading
#define AUTO_YAW_LOOK_AT_NEXT_WP 1 // point towards next waypoint (no pilot input accepted)
#define AUTO_YAW_LOOK_AT_LOCATION 2 // point towards a location held in yaw_look_at_WP (no pilot input accepted)
#define AUTO_YAW_LOOK_AT_HEADING 3 // point towards a particular angle (not pilot input accepted)
#define AUTO_YAW_LOOK_AHEAD 4 // point in the direction the copter is moving
#define AUTO_YAW_RESETTOARMEDYAW 5 // point towards heading at time motors were armed
#define YAW_ACRO 1 // pilot controlled yaw using rate controller
#define YAW_LOOK_AT_NEXT_WP 2 // point towards next waypoint (no pilot input accepted)
#define YAW_LOOK_AT_LOCATION 3 // point towards a location held in yaw_look_at_WP (no pilot input accepted)
#define YAW_CIRCLE 4 // point towards a location held in yaw_look_at_WP (no pilot input accepted)
#define YAW_LOOK_AT_HOME 5 // point towards home (no pilot input accepted)
#define YAW_LOOK_AT_HEADING 6 // point towards a particular angle (not pilot input accepted)
#define YAW_LOOK_AHEAD 7 // WARNING! CODE IN DEVELOPMENT NOT PROVEN
#define YAW_DRIFT 8 //
#define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed
#define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles
#define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates in body frame

44
ArduCopter/system.pde
View File

@ -406,18 +406,12 @@ static bool set_mode(uint8_t mode)
// check we have a GPS and at least one mission command (note the home position is always command 0)
if ((GPS_ok() && g.command_total > 1) || ignore_checks) {
success = auto_init(ignore_checks);
// roll-pitch, throttle and yaw modes will all be set by the first nav command
init_commands(); // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
}
break;
case CIRCLE:
if (GPS_ok() || ignore_checks) {
success = circle_init(ignore_checks);
set_roll_pitch_mode(CIRCLE_RP);
set_throttle_mode(CIRCLE_THR);
set_nav_mode(CIRCLE_NAV);
set_yaw_mode(CIRCLE_YAW);
}
break;
@ -430,19 +424,19 @@ static bool set_mode(uint8_t mode)
case POSITION:
if (GPS_ok() || ignore_checks) {
success = true;
set_yaw_mode(POSITION_YAW);
set_roll_pitch_mode(POSITION_RP);
set_throttle_mode(POSITION_THR);
//set_yaw_mode(POSITION_YAW);
//set_roll_pitch_mode(POSITION_RP);
//set_throttle_mode(POSITION_THR);
}
break;
case GUIDED:
if (GPS_ok() || ignore_checks) {
success = guided_init(ignore_checks);
set_yaw_mode(get_wp_yaw_mode(false));
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
set_nav_mode(GUIDED_NAV);
//set_yaw_mode(get_wp_yaw_mode(false));
//set_roll_pitch_mode(GUIDED_RP);
//set_throttle_mode(GUIDED_THR);
//set_nav_mode(GUIDED_NAV);
}
break;
@ -461,27 +455,27 @@ static bool set_mode(uint8_t mode)
case OF_LOITER:
if (g.optflow_enabled || ignore_checks) {
success = ofloiter_init(ignore_checks);
set_yaw_mode(OF_LOITER_YAW);
set_roll_pitch_mode(OF_LOITER_RP);
set_throttle_mode(OF_LOITER_THR);
set_nav_mode(OF_LOITER_NAV);
//set_yaw_mode(OF_LOITER_YAW);
//set_roll_pitch_mode(OF_LOITER_RP);
//set_throttle_mode(OF_LOITER_THR);
//set_nav_mode(OF_LOITER_NAV);
}
break;
case DRIFT:
success = drift_init(ignore_checks);
set_yaw_mode(YAW_DRIFT);
set_roll_pitch_mode(ROLL_PITCH_DRIFT);
set_nav_mode(NAV_NONE);
set_throttle_mode(DRIFT_THR);
//set_yaw_mode(YAW_DRIFT);
//set_roll_pitch_mode(ROLL_PITCH_DRIFT);
//set_nav_mode(NAV_NONE);
//set_throttle_mode(DRIFT_THR);
break;
case SPORT:
success = sport_init(ignore_checks);
set_yaw_mode(SPORT_YAW);
set_roll_pitch_mode(SPORT_RP);
set_throttle_mode(SPORT_THR);
set_nav_mode(NAV_NONE);
//set_yaw_mode(SPORT_YAW);
//set_roll_pitch_mode(SPORT_RP);
//set_throttle_mode(SPORT_THR);
//set_nav_mode(NAV_NONE);
// reset acro angle targets to current attitude
acro_roll = ahrs.roll_sensor;
acro_pitch = ahrs.pitch_sensor;