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

Fixed CIRCLE mode trig error 

made loiter_radius * 100 in Params
updated gains
This commit is contained in:
Jason Short 2011-09-16 16:54:45 -07:00
parent 8d0a2fae9b
commit 129c13e065
5 changed files with 40 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/ArduCopter.pde
View File

@ -345,6 +345,7 @@ static float cos_pitch_x = 1;
static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static long initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x;
static float boost; // used to give a little extra to maintain altitude
// Acro
@ -1043,17 +1044,29 @@ void update_roll_pitch_mode(void)
if(do_simple && new_radio_frame){
new_radio_frame = false;
//Serial.printf("1: %d, 2: %d ",g.rc_1.control_in, g.rc_2.control_in);
simple_timer++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_timer == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_timer > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_timer = 0;
}
// Rotate input by the initial bearing
control_roll = g.rc_1.control_in * sin_yaw_y + g.rc_2.control_in * cos_yaw_x;
control_pitch = g.rc_2.control_in * sin_yaw_y - g.rc_1.control_in * cos_yaw_x;
control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
//Serial.printf("\t1: %d, 2: %d\n",g.rc_1.control_in, g.rc_2.control_in);
}
switch(roll_pitch_mode){
case ROLL_PITCH_ACRO:
// Roll control
@ -1085,13 +1098,16 @@ void update_throttle_mode(void)
{
switch(throttle_mode){
case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){
g.rc_3.servo_out = g.rc_3.control_in + boost;
}else{
g.rc_3.servo_out = 0;
}
break;
case THROTTLE_HOLD:
// allow interactive changing of atitude
adjust_altitude();
// fall through
case THROTTLE_AUTO:
@ -1170,10 +1186,10 @@ static void update_navigation()
update_auto_yaw();
{
//circle_angle += dTnav; //1000 * (dTnav/1000);
circle_angle = wrap_360(target_bearing + 2000 + 18000);
circle_angle = wrap_360(target_bearing + 3000 + 18000);
target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(radians(90 - circle_angle)));
target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(radians(90 - circle_angle)));
target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100)));
target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100)));
}
// calc the lat and long error to the target

16
ArduCopter/Log.pde
View File

@ -558,7 +558,7 @@ static void Log_Write_Nav_Tuning()
static void Log_Read_Nav_Tuning()
{
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d\n"),
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), // distance
DataFlash.ReadByte(), // wp_verify_byte
DataFlash.ReadInt(), // target_bearing
@ -591,8 +591,6 @@ static void Log_Read_Nav_Tuning()
DataFlash.ReadInt(),
DataFlash.ReadInt()); //nav bearing
*/
}
@ -630,8 +628,8 @@ static void Log_Read_Control_Tuning()
"%d, %d\n"),
// Control
DataFlash.ReadInt(),
DataFlash.ReadInt(),
//DataFlash.ReadInt(),
//DataFlash.ReadInt(),
// yaw
DataFlash.ReadInt(),
@ -760,7 +758,7 @@ static void Log_Write_Attitude()
// Read an attitude packet
static void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, $d\n"),
Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(uint16_t)DataFlash.ReadInt(),
@ -821,15 +819,15 @@ static void Log_Read(int start_page, int end_page)
case 0:
if(data == HEAD_BYTE1) // Head byte 1
log_step++;
else
Serial.println(".");
break;
case 1:
if(data == HEAD_BYTE2) // Head byte 2
log_step++;
else
else{
log_step = 0;
Serial.println(".");
}
break;
case 2:

2
ArduCopter/Parameters.h
View File

@ -306,7 +306,7 @@ public:
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),

12
ArduCopter/config.h
View File

@ -392,7 +392,7 @@
// Attitude Control
//
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 3.6
# define STABILIZE_ROLL_P 4.0
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.02
@ -402,7 +402,7 @@
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 3.6
# define STABILIZE_PITCH_P 4.0
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.02
@ -474,7 +474,7 @@
# define LOITER_P .4 //
#endif
#ifndef LOITER_I
# define LOITER_I 0.01 //
# define LOITER_I 0.10 //
#endif
#ifndef LOITER_IMAX
# define LOITER_IMAX 12 // degrees°
@ -484,7 +484,7 @@
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
#endif
#ifndef NAV_I
# define NAV_I 0.12 // this
# define NAV_I 0.15 // this
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 20 // degrees
@ -654,8 +654,8 @@
# define WP_RADIUS_DEFAULT 3
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 10
#ifndef LOITER_RADIUS
# define LOITER_RADIUS 10
#endif
#ifndef ALT_HOLD_HOME

1
ArduCopter/motors_quad.pde
View File

@ -4,6 +4,7 @@
static void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;