AP_SmartAudio: Add configuration_finished and configuration_pending functions

This commit is contained in:
giacomo892 2021-05-27 17:47:27 +02:00 committed by Andrew Tridgell
parent 6df65e694d
commit 4dac9baa09
2 changed files with 13 additions and 5 deletions

View File

@ -68,6 +68,8 @@ bool AP_SmartAudio::init()
void AP_SmartAudio::loop() void AP_SmartAudio::loop()
{ {
AP_VideoTX &vtx = AP::vtx();
while (!hal.scheduler->is_system_initialized()) { while (!hal.scheduler->is_system_initialized()) {
hal.scheduler->delay(100); hal.scheduler->delay(100);
} }
@ -141,13 +143,15 @@ void AP_SmartAudio::loop()
if (AP::vtx().have_params_changed() ||_vtx_power_change_pending if (AP::vtx().have_params_changed() ||_vtx_power_change_pending
|| _vtx_freq_change_pending || _vtx_options_change_pending) { || _vtx_freq_change_pending || _vtx_options_change_pending) {
update_vtx_params(); update_vtx_params();
_vtx_gcs_pending = true; set_configuration_pending(true);
vtx.set_configuration_finished(false);
// we've tried to udpate something, re-request the settings so that they // we've tried to udpate something, re-request the settings so that they
// are reflected correctly // are reflected correctly
request_settings(); request_settings();
} else if (_vtx_gcs_pending) { } else if (is_configuration_pending()) {
AP::vtx().announce_vtx_settings(); AP::vtx().announce_vtx_settings();
_vtx_gcs_pending = false; set_configuration_pending(false);
vtx.set_configuration_finished(true);
} }
} }
} }
@ -237,9 +241,10 @@ void AP_SmartAudio::update_vtx_params()
break; break;
} }
} }
} else {
vtx.set_configuration_finished(true);
} }
} }
/** /**
* Sends an SmartAudio Command to the vtx, waits response on the update event * Sends an SmartAudio Command to the vtx, waits response on the update event
* @param frameBuffer frameBuffer to send over the wire * @param frameBuffer frameBuffer to send over the wire

View File

@ -191,7 +191,7 @@ private:
bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
bool _vtx_power_change_pending; bool _vtx_power_change_pending;
bool _vtx_options_change_pending; bool _vtx_options_change_pending;
bool _vtx_gcs_pending; bool _vtx_changes_pending;
bool _vtx_use_set_freq; // should frequency set by band/channel or frequency bool _vtx_use_set_freq; // should frequency set by band/channel or frequency
// value for current baud adjust // value for current baud adjust
@ -265,5 +265,8 @@ private:
// change baud automatically when request-response fails many times // change baud automatically when request-response fails many times
void update_baud_rate(); void update_baud_rate();
void set_configuration_pending(bool pending) { _vtx_changes_pending = pending; }
bool is_configuration_pending(){ return _vtx_changes_pending;}
}; };
#endif #endif