mirror of https://github.com/ArduPilot/ardupilot
Copter: Leonard Hall's inertial nav ver 2
This commit is contained in:
Randy Mackay
parent
7ab3d93ed4
commit
9a09f086d2
|
|
@ -1599,7 +1599,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
|
|||
roll_pitch_initialised = true;
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_VELOCITY:
|
||||
case ROLL_PITCH_LOITER_POS_VEL:
|
||||
// require gps lock
|
||||
if( ap.home_is_set ) {
|
||||
roll_pitch_initialised = true;
|
||||
|
|
@ -1710,7 +1710,7 @@ void update_roll_pitch_mode(void)
|
|||
roll_pitch_toy();
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_VELOCITY:
|
||||
case ROLL_PITCH_LOITER_POS_VEL:
|
||||
// apply SIMPLE mode transform
|
||||
if(ap.simple_mode && ap_system.new_radio_frame) {
|
||||
update_simple_mode();
|
||||
|
|
@ -1718,8 +1718,8 @@ void update_roll_pitch_mode(void)
|
|||
control_roll = g.rc_1.control_in;
|
||||
control_pitch = g.rc_2.control_in;
|
||||
|
||||
// pilot controls velocity up to 4.5 m/s
|
||||
get_roll_pitch_vel(-control_pitch/10, control_roll/10);
|
||||
// Set max velocity to 2.5 m/s for conservative start
|
||||
get_loiter_pos_vel(-control_pitch/(1*4.5), control_roll/(1*4.5));
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -216,42 +216,20 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
|
|||
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
|
||||
}
|
||||
|
||||
|
||||
// Accel controled Roll and Pitch
|
||||
static void
|
||||
get_roll_pitch_accel(int16_t accel_forward, int16_t accel_right)
|
||||
get_loiter_accel(int16_t accel_req_forward, int16_t accel_req_right)
|
||||
{
|
||||
static float z_accel_meas = 0; // The acceleration error in cm.
|
||||
static float z_accel_meas = 0; // The acceleration error in cm.
|
||||
static float accel_forward = 0; // The acceleration error in cm.
|
||||
static float accel_right = 0; // The acceleration error in cm.
|
||||
int16_t desired_roll;
|
||||
int16_t desired_pitch;
|
||||
|
||||
// int16_t forward_accel_meas;
|
||||
// int16_t right_accel_meas;
|
||||
|
||||
// int16_t accel_forward_error;
|
||||
// int16_t accel_right_error;
|
||||
|
||||
Vector3f accel = ins.get_accel();
|
||||
|
||||
// calculate accel error and Filter with fc = 2 Hz
|
||||
z_accel_meas = z_accel_meas + 0.11164 * (accel.z*100 - z_accel_meas);
|
||||
|
||||
// Matrix3f dcm_matrix = ahrs.get_dcm_matrix();
|
||||
|
||||
// Calculate Earth Frame Z acceleration
|
||||
// forward_accel_meas = (accel.x * cos_pitch_x + accel.y * sin_roll * sin_pitch + accel.z * cos_roll_x * sin_pitch)* 100.0;
|
||||
// right_accel_meas = (accel.y * cos_roll_x - accel.z * sin_roll)* 100.0;
|
||||
// accel_forward_error = accel_forward - forward_accel_meas;
|
||||
// accel_right_error = accel_right - right_accel_meas;
|
||||
|
||||
// forward_p = g.pid_loiter_accel_forward.get_p(lat_speed_error);
|
||||
// forward_i = g.pid_loiter_accel_forward.get_i(lat_speed_error, .01);
|
||||
// forward_d = g.pid_loiter_accel_forward.get_d(lat_speed_error, .01);
|
||||
|
||||
// right_p = g.pid_loiter_accel_right.get_p(lon_speed_error);
|
||||
// right_i = g.pid_loiter_accel_right.get_i(lon_speed_error, .01);
|
||||
// right_d = g.pid_loiter_accel_right.get_d(lon_speed_error, .01);
|
||||
z_accel_meas = -AP_INTERTIALNAV_GRAVITY * 100;
|
||||
|
||||
accel_forward = accel_forward + 0.11164 * (accel_req_forward - accel_forward);
|
||||
accel_right = accel_right + 0.11164 * (accel_req_right - accel_right);
|
||||
|
||||
desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
|
||||
desired_pitch = constrain((-accel_forward/(-z_accel_meas*cos_roll_x))*(18000/M_PI), -4500, 4500);
|
||||
|
|
@ -259,119 +237,165 @@ get_roll_pitch_accel(int16_t accel_forward, int16_t accel_right)
|
|||
get_stabilize_roll(desired_roll);
|
||||
get_stabilize_pitch(desired_pitch);
|
||||
|
||||
// for logging purposes only
|
||||
// update nav_roll and nav_pitch for reporting purposes
|
||||
nav_roll = desired_roll;
|
||||
nav_pitch = desired_pitch;
|
||||
//Serial.printf("Test: accel_x:%4.2f accel_y:%4.2f z_accel_meas:%4.2f desired_roll:%4.2f desired_pitch:%4.2f M_PI:%4.2f \n", 1.0*accel_x, 1.0*accel_y, 1.0*z_accel_meas, 1.0*control_roll, 1.0*control_pitch, 1.0*M_PI);
|
||||
}
|
||||
|
||||
|
||||
// Velocity controled Roll and Pitch
|
||||
static void
|
||||
get_roll_pitch_vel(int16_t vel_x, int16_t vel_y)
|
||||
get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
|
||||
{
|
||||
static float lat_speed_error = 0; // The velocity in cm/s.
|
||||
static float lon_speed_error = 0; // The velocity in cm/s.
|
||||
float accel_forward;
|
||||
float accel_right;
|
||||
|
||||
float lat_speed = inertial_nav.get_latitude_velocity();
|
||||
float lon_speed = inertial_nav.get_longitude_velocity();
|
||||
accel_forward = accel_lat*cos_yaw + accel_lon*sin_yaw;
|
||||
accel_right = -accel_lat*sin_yaw + accel_lon*cos_yaw;
|
||||
|
||||
int16_t lat_vel;
|
||||
int16_t lon_vel;
|
||||
get_loiter_accel(accel_forward, accel_right);
|
||||
}
|
||||
|
||||
int16_t accel_x;
|
||||
int16_t accel_y;
|
||||
float accel_total;
|
||||
|
||||
// Velocity controled Roll and Pitch
|
||||
#define MAX_LOITER_VEL_ACCEL 600
|
||||
static void
|
||||
get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon)
|
||||
{
|
||||
static float speed_error_lat = 0; // The velocity in cm/s.
|
||||
static float speed_error_lon = 0; // The velocity in cm/s.
|
||||
|
||||
float speed_lat = inertial_nav.get_latitude_velocity();
|
||||
float speed_lon = inertial_nav.get_longitude_velocity();
|
||||
|
||||
int32_t accel_lat;
|
||||
int32_t accel_lon;
|
||||
int32_t accel_total;
|
||||
|
||||
int16_t lat_p,lat_i,lat_d;
|
||||
int16_t lon_p,lon_i,lon_d;
|
||||
|
||||
lat_vel = vel_x*cos_yaw - vel_y*sin_yaw;
|
||||
lon_vel = vel_x*sin_yaw + vel_y*cos_yaw;
|
||||
|
||||
// calculate vel error and Filter with fc = 2 Hz
|
||||
lat_speed_error = lat_speed_error + 0.11164 * ((lat_vel -lat_speed) - lat_speed_error);
|
||||
lon_speed_error = lon_speed_error + 0.11164 * ((lon_vel - lon_speed) - lon_speed_error);
|
||||
speed_error_lat = speed_error_lat + 0.11164 * ((vel_lat - speed_lat) - speed_error_lat);
|
||||
speed_error_lon = speed_error_lon + 0.11164 * ((vel_lon - speed_lon) - speed_error_lon);
|
||||
|
||||
lat_p = g.pid_loiter_rate_lat.get_p(lat_speed_error);
|
||||
lat_i = g.pid_loiter_rate_lat.get_i(lat_speed_error, .01);
|
||||
lat_d = g.pid_loiter_rate_lat.get_d(lat_speed_error, .01);
|
||||
lat_p = g.pid_loiter_rate_lat.get_p(speed_error_lat);
|
||||
lat_i = g.pid_loiter_rate_lat.get_i(speed_error_lat, .01);
|
||||
lat_d = g.pid_loiter_rate_lat.get_d(speed_error_lat, .01);
|
||||
|
||||
lon_p = g.pid_loiter_rate_lon.get_p(lon_speed_error);
|
||||
lon_i = g.pid_loiter_rate_lon.get_i(lon_speed_error, .01);
|
||||
lon_d = g.pid_loiter_rate_lon.get_d(lon_speed_error, .01);
|
||||
lon_p = g.pid_loiter_rate_lon.get_p(speed_error_lon);
|
||||
lon_i = g.pid_loiter_rate_lon.get_i(speed_error_lon, .01);
|
||||
lon_d = g.pid_loiter_rate_lon.get_d(speed_error_lon, .01);
|
||||
|
||||
accel_x = (lat_p+lat_i+lat_d)*cos_yaw + (lon_p+lon_i+lon_d)*sin_yaw;
|
||||
accel_y = -(lat_p+lat_i+lat_d)*sin_yaw + (lon_p+lon_i+lon_d)*cos_yaw;
|
||||
accel_lat = (lat_p+lat_i+lat_d);
|
||||
accel_lon = (lon_p+lon_i+lon_d);
|
||||
|
||||
accel_total = safe_sqrt(accel_x*accel_x + accel_y*accel_y);
|
||||
accel_total = safe_sqrt(accel_lat*accel_lat + accel_lon*accel_lon);
|
||||
|
||||
if( accel_total > 300 ) {
|
||||
accel_x = 300 * accel_x/accel_total;
|
||||
accel_y = 300 * accel_x/accel_total;
|
||||
if( accel_total > MAX_LOITER_VEL_ACCEL ) {
|
||||
accel_lat = MAX_LOITER_VEL_ACCEL * accel_lat/accel_total;
|
||||
accel_lon = MAX_LOITER_VEL_ACCEL * accel_lon/accel_total;
|
||||
}
|
||||
|
||||
get_roll_pitch_accel(accel_x, accel_y);
|
||||
|
||||
// log output if PID logging is on and we are tuning the yaw
|
||||
/*pid_log_counter++;
|
||||
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
|
||||
pid_log_counter = 0;
|
||||
Log_Write_PID(1, lat_speed_error, lat_p, lat_i, lat_d, (lat_p+lat_i+lat_d), accel_x);
|
||||
Log_Write_PID(2, lon_speed_error, lon_p, lon_i, lon_d, (lon_p+lon_i+lon_d), accel_y);
|
||||
}*/
|
||||
|
||||
//Serial.printf("Test: vel_x:%4.2f vel_y:%4.2f sin_yaw:%4.2f cos_yaw:%4.2f accel_x:%4.2f accel_y:%4.2f \n", 1.0*vel_x, 1.0*vel_y, 1.0*sin_yaw, 1.0*cos_yaw, 1.0*accel_x, 1.0*accel_y);
|
||||
get_loiter_accel_lat_lon(accel_lat, accel_lon);
|
||||
}
|
||||
|
||||
|
||||
// Velocity controled Roll and Pitch
|
||||
static void
|
||||
get_roll_pitch_pos(int32_t target_lat, int32_t target_lon)
|
||||
get_loiter_vel(int16_t vel_forward, int16_t vel_right)
|
||||
{
|
||||
static float lat_error;
|
||||
int16_t desired_lat_vel;
|
||||
int16_t vel_lat;
|
||||
int16_t vel_lon;
|
||||
|
||||
static float lon_error;
|
||||
int16_t desired_lon_vel;
|
||||
vel_lat = vel_forward*cos_yaw - vel_right*sin_yaw;
|
||||
vel_lon = vel_forward*sin_yaw + vel_right*cos_yaw;
|
||||
|
||||
float total_error;
|
||||
int16_t sqrt_vel;
|
||||
get_loiter_vel(vel_lat, vel_lon);
|
||||
}
|
||||
|
||||
int16_t vel_x;
|
||||
int16_t vel_y;
|
||||
|
||||
// Velocity controled Roll and Pitch
|
||||
#define MAX_LOITER_POS_VELOCITY 1500
|
||||
#define MAX_LOITER_POS_ACCEL 500
|
||||
|
||||
static void
|
||||
get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon)
|
||||
{
|
||||
static float dist_error_lat;
|
||||
int32_t desired_vel_lat;
|
||||
|
||||
static float dist_error_lon;
|
||||
int32_t desired_vel_lon;
|
||||
|
||||
int32_t dist_error_total;
|
||||
|
||||
int16_t vel_sqrt;
|
||||
int32_t vel_total;
|
||||
|
||||
int16_t linear_distance; // the distace we swap between linear and sqrt.
|
||||
|
||||
// calculate distance error and Filter with fc = 2 Hz
|
||||
lat_error = lat_error + 0.11164 * ((target_lat - inertial_nav.get_latitude_diff()) - lat_error);
|
||||
lon_error = lon_error + 0.11164 * ((target_lon - inertial_nav.get_longitude_diff()) - lon_error);
|
||||
dist_error_lat = dist_error_lat + 0.11164 * ((target_lat - inertial_nav.get_latitude_diff()) - dist_error_lat);
|
||||
dist_error_lon = dist_error_lon + 0.11164 * ((target_lon - inertial_nav.get_longitude_diff()) - dist_error_lon);
|
||||
|
||||
linear_distance = 250/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP());
|
||||
linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP());
|
||||
|
||||
total_error = safe_sqrt(lat_error*lat_error + lon_error*lon_error);
|
||||
if( total_error > 2*linear_distance ) {
|
||||
sqrt_vel = constrain(safe_sqrt(2*250*(total_error-linear_distance)),0,1000);
|
||||
desired_lat_vel = sqrt_vel * lat_error/total_error;
|
||||
desired_lon_vel = sqrt_vel * lon_error/total_error;
|
||||
dist_error_total = safe_sqrt(dist_error_lat*dist_error_lat + dist_error_lon*dist_error_lon);
|
||||
if( dist_error_total > 2*linear_distance ) {
|
||||
vel_sqrt = constrain(safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance)),0,1000);
|
||||
desired_vel_lat = vel_sqrt * dist_error_lat/dist_error_total;
|
||||
desired_vel_lon = vel_sqrt * dist_error_lon/dist_error_total;
|
||||
}else{
|
||||
desired_lat_vel = g.pi_loiter_lat.get_p(lat_error);
|
||||
desired_lon_vel = g.pi_loiter_lon.get_p(lon_error);
|
||||
desired_vel_lat = g.pi_loiter_lat.get_p(dist_error_lat);
|
||||
desired_vel_lon = g.pi_loiter_lon.get_p(dist_error_lon);
|
||||
}
|
||||
|
||||
vel_x = constrain(desired_lat_vel*cos_yaw + desired_lon_vel*sin_yaw, -3000, 3000);
|
||||
vel_y = constrain(-desired_lat_vel*sin_yaw + desired_lon_vel*cos_yaw, -3000, 3000);
|
||||
vel_total = safe_sqrt(desired_vel_lat*desired_vel_lat + desired_vel_lon*desired_vel_lon);
|
||||
if( vel_total > MAX_LOITER_POS_VELOCITY ) {
|
||||
desired_vel_lat = MAX_LOITER_POS_VELOCITY * desired_vel_lat/vel_total;
|
||||
desired_vel_lon = MAX_LOITER_POS_VELOCITY * desired_vel_lon/vel_total;
|
||||
}
|
||||
|
||||
get_roll_pitch_vel(vel_x, vel_y);
|
||||
get_loiter_vel_lat_lon(desired_vel_lat, desired_vel_lon);
|
||||
|
||||
// log output if PID logging is on and we are tuning the yaw
|
||||
/*pid_log_counter++;
|
||||
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
|
||||
pid_log_counter = 0;
|
||||
Log_Write_PID(3, lat_error, lon_error, desired_lat_vel, desired_lon_vel, total_error, linear_distance);
|
||||
}*/
|
||||
// Serial.printf("Test: target_lat:%4.2f inertial_nav.get_latitude_diff():%4.2f linear_distance:%4.2f desired_lat_vel:%4.2f vel_x:%4.2f \n", 1.0*target_lat, 1.0*inertial_nav.get_latitude_diff(), 1.0*linear_distance, 1.0*desired_lat_vel, 1.0*vel_x);
|
||||
// Serial.printf("Test: target_lon:%4.2f inertial_nav.get_longitude_diff():%4.2f linear_distance:%4.2f desired_lon_vel:%4.2f vel_y:%4.2f \n", 1.0*target_lon, 1.0*inertial_nav.get_longitude_diff(), 1.0*linear_distance, 1.0*desired_lon_vel, 1.0*vel_y);
|
||||
}
|
||||
|
||||
|
||||
#define MAX_LOITER_POS_VEL_VELOCITY 1000
|
||||
// Velocity controled Roll and Pitch
|
||||
static void
|
||||
get_loiter_pos_vel(int16_t vel_forward, int16_t vel_right)
|
||||
{
|
||||
static float pos_lat;
|
||||
static float pos_lon;
|
||||
|
||||
int32_t vel_lat;
|
||||
int32_t vel_lon;
|
||||
int32_t vel_total;
|
||||
static uint32_t last_call_ms = 0; // the last time this controller was called
|
||||
uint32_t now = millis();
|
||||
|
||||
vel_lat = vel_forward*cos_yaw - vel_right*sin_yaw;
|
||||
vel_lon = vel_forward*sin_yaw + vel_right*cos_yaw;
|
||||
|
||||
// reset target pos if this controller has just been engaged
|
||||
if( now - last_call_ms > 100 ) {
|
||||
pos_lat = inertial_nav.get_latitude_diff();
|
||||
pos_lon = inertial_nav.get_longitude_diff();
|
||||
}
|
||||
last_call_ms = now;
|
||||
|
||||
vel_total = safe_sqrt(vel_lat*vel_lat + vel_lon*vel_lon);
|
||||
if( vel_total > MAX_LOITER_POS_VEL_VELOCITY ) {
|
||||
vel_lat = MAX_LOITER_POS_VEL_VELOCITY * vel_lat/vel_total;
|
||||
vel_lon = MAX_LOITER_POS_VEL_VELOCITY * vel_lon/vel_total;
|
||||
}
|
||||
|
||||
pos_lat += vel_lat * 0.01;
|
||||
pos_lon += vel_lon * 0.01;
|
||||
|
||||
get_loiter_pos_lat_lon(pos_lat, pos_lon);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -898,7 +922,6 @@ static int16_t get_angle_boost(int16_t throttle)
|
|||
temp = constrain(temp, .5, 1.0);
|
||||
temp = constrain(9000-max(labs(roll_axis),labs(pitch_axis)), 0, 3000) / (3000 * temp);
|
||||
throttle_out = constrain((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000);
|
||||
//Serial.printf("Thin:%4.2f sincos:%4.2f temp:%4.2f roll_axis:%4.2f Out:%4.2f \n", 1.0*throttle, 1.0*cos_pitch_x * cos_roll_x, 1.0*temp, 1.0*roll_axis, 1.0*constrain((float)value * temp, 0, 1000));
|
||||
|
||||
// to allow logging of angle boost
|
||||
angle_boost = throttle_out - throttle;
|
||||
|
|
|
|||
|
|
@ -25,12 +25,12 @@
|
|||
#define YAW_TOY 7 // THOR This is the Yaw mode
|
||||
|
||||
|
||||
#define ROLL_PITCH_STABLE 0
|
||||
#define ROLL_PITCH_ACRO 1
|
||||
#define ROLL_PITCH_AUTO 2
|
||||
#define ROLL_PITCH_STABLE_OF 3
|
||||
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode
|
||||
#define ROLL_PITCH_VELOCITY 5 // pilot inputs the desired horizontal velocities
|
||||
#define ROLL_PITCH_STABLE 0
|
||||
#define ROLL_PITCH_ACRO 1
|
||||
#define ROLL_PITCH_AUTO 2
|
||||
#define ROLL_PITCH_STABLE_OF 3
|
||||
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode
|
||||
#define ROLL_PITCH_LOITER_POS_VEL 5 // pilot inputs the desired horizontal velocities
|
||||
|
||||
#define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors
|
||||
#define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation
|
||||
|
|
|
|||
Loading…
Reference in New Issue