Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: surf tracking, do-land use inav alt

Browse Source 
Also CTUN logging of inav alt switched to get directly from inertial nav
lib instead of using current_loc.alt
This commit is contained in:
Randy Mackay 2015-02-10 21:28:30 +09:00
parent cb66bf8b98
commit 7e11ec9a6f
3 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/Attitude.pde
View File

@ -240,12 +240,13 @@ static float get_throttle_surface_tracking(int16_t target_rate, float current_al
static uint32_t last_call_ms = 0; static uint32_t last_call_ms = 0;
float distance_error; float distance_error;
float velocity_correction; float velocity_correction;
float current_alt = inertial_nav.get_altitude();
uint32_t now = millis(); uint32_t now = millis();
// reset target altitude if this controller has just been engaged // reset target altitude if this controller has just been engaged
if (now - last_call_ms > SONAR_TIMEOUT_MS) { if (now - last_call_ms > SONAR_TIMEOUT_MS) {
target_sonar_alt = sonar_alt + current_alt_target - current_loc.alt; target_sonar_alt = sonar_alt + current_alt_target - current_alt;
} }
last_call_ms = now; last_call_ms = now;
@ -259,7 +260,7 @@ static float get_throttle_surface_tracking(int16_t target_rate, float current_al
target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z()); target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z());
// calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations) // calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_loc.alt); distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_alt);
velocity_correction = distance_error * g.sonar_gain; velocity_correction = distance_error * g.sonar_gain;
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX); velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);

2
ArduCopter/Log.pde
View File

@ -315,7 +315,7 @@ static void Log_Write_Control_Tuning()
angle_boost : attitude_control.angle_boost(), angle_boost : attitude_control.angle_boost(),
throttle_out : g.rc_3.servo_out, throttle_out : g.rc_3.servo_out,
desired_alt : pos_control.get_alt_target() / 100.0f, desired_alt : pos_control.get_alt_target() / 100.0f,
inav_alt : current_loc.alt / 100.0f, inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt, baro_alt : baro_alt,
desired_sonar_alt : (int16_t)target_sonar_alt, desired_sonar_alt : (int16_t)target_sonar_alt,
sonar_alt : sonar_alt, sonar_alt : sonar_alt,

2
ArduCopter/commands_logic.pde
View File

@ -345,7 +345,7 @@ static void do_land(const AP_Mission::Mission_Command& cmd)
// calculate and set desired location above landing target // calculate and set desired location above landing target
Vector3f pos = pv_location_to_vector(cmd.content.location); Vector3f pos = pv_location_to_vector(cmd.content.location);
pos.z = current_loc.alt; pos.z = inertial_nav.get_altitude();
auto_wp_start(pos); auto_wp_start(pos);
}else{ }else{
// set landing state // set landing state