aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: port betaflight in-flight fft analysis to arducopter and expose as a log message

Browse Source 
add harmonic notch tracking mode
call AP_GyroFFT::update_freq_hover() from update_throttle_hover()
move gyrofft configuration and control to AP_Vehicle
move fft logging to fft library
This commit is contained in:
Andy Piper 2019-07-18 21:19:08 +02:00 committed by Andrew Tridgell
parent 6f5b991f28
commit 4c1816a983
6 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Attitude.cpp
View File

@ -64,6 +64,9 @@ void Copter::update_throttle_hover()
labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
// Can we set the time constant automatically
motors->update_throttle_hover(0.01f);
#if HAL_GYROFFT_ENABLED
gyro_fft.update_freq_hover(0.01f, motors->get_throttle_out());
#endif
}
#endif
}

4
ArduCopter/Copter.cpp
View File

@ -161,6 +161,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50),
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75),
#if RPM_ENABLED == ENABLED
SCHED_TASK(rpm_update, 40, 200),
@ -575,6 +576,9 @@ void Copter::update_altitude()
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#endif
}
}

6
ArduCopter/Log.cpp
View File

@ -20,7 +20,6 @@ struct PACKED log_Control_Tuning {
float terr_alt;
int16_t target_climb_rate;
int16_t climb_rate;
float dynamic_notch_freq;
};
// Write a control tuning packet
@ -60,8 +59,7 @@ void Copter::Log_Write_Control_Tuning()
rangefinder_alt : surface_tracking.get_dist_for_logging(),
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,
climb_rate : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t
dynamic_notch_freq : ins.get_gyro_dynamic_notch_center_freq_hz()
climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
@ -456,7 +454,7 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B000BB-" },
"CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B000BB" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),

2
ArduCopter/RC_Channel.cpp
View File

@ -556,7 +556,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
break;
}
break;
default:
RC_Channel::do_aux_function(ch_option, ch_flag);
break;

7
ArduCopter/system.cpp
View File

@ -133,6 +133,7 @@ void Copter::init_ardupilot()
attitude_control->parameter_sanity_check();
pos_control->set_dt(scheduler.get_loop_period_s());
// init the optical flow sensor
init_optflow();
@ -291,6 +292,12 @@ void Copter::update_dynamic_notch()
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref));
break;
#endif
#if HAL_GYROFFT_ENABLED
case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
// set the harmonic notch filter frequency scaled on measured frequency
ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
break;
#endif
case HarmonicNotchDynamicMode::Fixed: // static
default:

1
libraries/Filter/HarmonicNotchFilter.h
View File

@ -64,6 +64,7 @@ enum class HarmonicNotchDynamicMode {
UpdateThrottle = 1,
UpdateRPM = 2,
UpdateBLHeli = 3,
UpdateGyroFFT = 4,
};
/*