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

Copter: add panorama to CIRCLE mode 

Yaw will slowly rotate if CIRCLE_RADIUS is set to zero
Circle center is projected forward CIRCLE_RADIUS from current position
and heading
This commit is contained in:
Randy Mackay 2013-03-22 17:38:07 +09:00
parent 60060d8184
commit 1516972eaa
7 changed files with 94 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/ArduCopter.pde
View File

@ -1442,6 +1442,15 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
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;
@ -1502,9 +1511,18 @@ void update_yaw_mode(void)
break;
case YAW_LOOK_AT_LOCATION:
// point towards a location held in yaw_look_at_WP (no pilot input accepted)
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
// 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( g.rc_4.control_in != 0 ) {
set_yaw_mode(YAW_HOLD);
}
break;
case YAW_CIRCLE:
// 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( g.rc_4.control_in != 0 ) {

41
ArduCopter/Attitude.pde
View File

@ -682,6 +682,47 @@ get_of_pitch(int32_t input_pitch)
* yaw controllers
*************************************************************/
// get_look_at_yaw - updates bearing to look at center of circle or do a panorama
// should be called at 100hz
static void get_circle_yaw()
{
static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz
// if circle radius is zero do panorama
if( g.circle_radius == 0 ) {
// slew yaw towards circle angle
nav_yaw = get_yaw_slew(nav_yaw, ToDeg(circle_angle)*100, AUTO_YAW_SLEW_RATE);
}else{
look_at_yaw_counter++;
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
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
}
// call stabilize yaw controller
get_stabilize_yaw(nav_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 uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz
look_at_yaw_counter++;
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
nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
}
static void get_look_ahead_yaw(int16_t pilot_yaw)
{
// Commanded Yaw to automatically look ahead.

1
ArduCopter/commands_logic.pde
View File

@ -372,7 +372,6 @@ static void do_circle()
}
// set yaw to point to center of circle
yaw_look_at_WP = circle_center;
set_yaw_mode(CIRCLE_YAW);
// set angle travelled so far to zero

2
ArduCopter/config.h
View File

@ -593,7 +593,7 @@
// CIRCLE Mode
#ifndef CIRCLE_YAW
# define CIRCLE_YAW YAW_LOOK_AT_LOCATION
# define CIRCLE_YAW YAW_CIRCLE
#endif
#ifndef CIRCLE_RP

9
ArduCopter/defines.h
View File

@ -19,10 +19,11 @@
#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_LOOK_AT_HOME 4 // point towards home (no pilot input accepted)
#define YAW_LOOK_AT_HEADING 5 // point towards a particular angle (not pilot input accepted)
#define YAW_LOOK_AHEAD 6 // WARNING! CODE IN DEVELOPMENT NOT PROVEN
#define YAW_TOY 7 // THOR This is the Yaw mode
#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_TOY 8 // THOR This is the Yaw mode
#define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles

47
ArduCopter/navigation.pde
View File

@ -81,12 +81,6 @@ static void calc_distance_and_bearing()
home_distance = 0;
home_bearing = 0;
}
// calculate bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
// To-Do: move this to the look-at-waypoint yaw controller
if( yaw_mode == YAW_LOOK_AT_LOCATION ) {
yaw_look_at_WP_bearing = pv_get_bearing_cd(curr, yaw_look_at_WP);
}
}
// run_autopilot - highest level call to process mission commands
@ -279,25 +273,31 @@ static bool waypoint_valid(Location &wp)
// circle_set_center -- set circle controller's center position and starting angle
static void
circle_set_center(const Vector3f pos_vec, float heading_in_radians)
circle_set_center(const Vector3f current_position, float heading_in_radians)
{
// set circle center
circle_center = pos_vec;
// set circle center to circle_radius ahead of current position
circle_center.x = current_position.x + (float)g.circle_radius * 100 * sin_yaw_y;
circle_center.y = current_position.y + (float)g.circle_radius * 100 * cos_yaw_x;
// set starting angle to current heading - 180 degrees
circle_angle = heading_in_radians-ToRad(180);
if( circle_angle > 180 ) {
circle_angle -= 180;
}
if( circle_angle < -180 ) {
circle_angle -= 180;
// if we are doing a panorama set the circle_angle to the current heading
if( g.circle_radius == 0 ) {
circle_angle = heading_in_radians;
}else{
// set starting angle to current heading - 180 degrees
circle_angle = heading_in_radians-ToRad(180);
if( circle_angle > 180 ) {
circle_angle -= 180;
}
if( circle_angle < -180 ) {
circle_angle -= 180;
}
}
// initialise other variables
circle_angle_total = 0;
}
// circle_get_pos - circle position controller's main call which in turn calls loiter controller with updated target position
// update_circle - circle position controller's main call which in turn calls loiter controller with updated target position
static void
update_circle(float dt)
{
@ -317,12 +317,15 @@ update_circle(float dt)
// update the total angle travelled
circle_angle_total += angle_delta;
// calculate target position
circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle);
circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle);
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if( g.circle_radius != 0.0 ) {
// calculate target position
circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle);
circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle);
// re-use loiter position controller
wp_nav.set_loiter_target(circle_target);
// re-use loiter position controller
wp_nav.set_loiter_target(circle_target);
}
// call loiter controller
wp_nav.update_loiter();

3
ArduCopter/system.pde
View File

@ -426,11 +426,10 @@ static void set_mode(uint8_t mode)
case CIRCLE:
ap.manual_throttle = false;
ap.manual_attitude = false;
// set yaw to point to center of circle
set_yaw_mode(CIRCLE_YAW);
set_roll_pitch_mode(CIRCLE_RP);
set_throttle_mode(CIRCLE_THR);
set_nav_mode(CIRCLE_NAV);
set_yaw_mode(CIRCLE_YAW);
break;
case LOITER: