SmartAudio library implementation

[wapophis]

This is smartaudio protocol implementatation to deal with the vtx directly throught serial port interface.

That's is a wip, is tested with 2.0 spec. The idea behind this PR is continue working with using this PR as a continuous improvement process.

Help debugging in 1.0 and 2.1 vtxs is welcome

The setup is the following:

• Wiring: Connect the tx of one spare uart of your fc to the sma pin in the vtx. Also connect the gnd from uart to the gnd of the vtx, be carefull to not mistake with the - pole from power.
• Setup the uart port protocol to value 35 to enable sma protocol in this uart, baud setup is in the sma driver settled to 4800 baud as spec says. This is made setting the value into the SERIAL_<<UART_NUMBER>> protocol param. Setup uart options to half-duplex.
• Enable vtx setup setting to 1 the para VTX_ENABLE, save and reboot.
• Enable sma protocol in vtx setting the VTX_SMA_ENABLE PARAM TO 1, save and reboot.
• Setup your desired params for band,chan and power in the VTX_BAND,VTX_CHAN,VTX_POWER.
• Setup the additional options which affects pitmode, or unlocking features for the vtx.
• Save and reboot.
  When the vtx has power on, the sma driver will send to the hw-vtx the commands needed to sincro your vtx params config with the hw-vtx.
  Thats all for now.

[andyp1per]

@wapophis thanks for working on this! The PR will need quite a few changes to be in a mergeable state, here are the high level changes I think are needed - probably not worth getting into the details until we get a bit closer:

• There is no need for the frontend/backend split, this makes the code unnecessarily complex and large - it should be possible to consolidate into a much smaller set of files. Flash usage is a priority for us and at the moment I suspect the flash usage of this library will be large.
• You should remove AP_Vtx_Backend and use libraries/AP_RCTelemetry/AP_VideoTX - there is a lot of duplicated code that can be removed in favour of the existing code (or modify the existing code)
• The betaflight C code for smartaudio_protocol needs to be converted into C++ and merged into the AP_SmartAudio implementation
• The update loop and config should be put in libraries/AP_Vehicle rather than Copter. AP_VideoTX already has an update look so I suspect that this could just call into the SA code.
• Any extra config should be incorporated into AP_VideoTX and there rest re-used from AP_VideoTX
• You should use a define in the smart audio library to enable/disable rather than putting in Copter - have a look at libraries/AP_Camera/AP_RunCam.h for an example
• If I read the code correctly it is attempting to be a state machine - I think this probably makes the code more complicated than it needs to be and could be removed in favour of a thread. betaflight does not have support for threads which is why they have done it this way - but a state machine is also possible and may be ok once some of the other complexity has been removed.
• You should remove all the whitespace changes from files unrelated to this PR.
• All commit messages should start with the module the commit applies to. There should be one commit per module.

I'm happy to help you with this if this does not make sense.

[wapophis]

Working with the AP_Vehicle i see that the params root is defined here and not in the Parameters.cpp. Is correct to move the root param definition to the AP_Vehicle ?
One more question is about the needs to use the #ifdef, your impl from vtx is setted directly without def check, is this the usual way or is exceptional ?.
Thanks in advance.

[andyp1per]

@wapophis - I'm not quite sure what you mean but what you have done is correct - there is no need to modify Parameters.cpp.

Yeah VTX is protocol independent and small which is why it is universally enabled. The only implemented protocol so far is CRSF which does have the appropriate ifdef

[wapophis]

I'm thinking about the thread strategy you refer, using the The AP_Scheduler system is the way ?
In the RCTelemetry impl i see a custom impl using a querybuffer (maybe circular) with a custom sheduler.

[andyp1per]

@wapophis - don't use the scheduler in RCTelemetry, that's to fit telemetry into a limited bandwidth link. The way to create a thread is something like:

    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_GyroFFT::update_thread, void), "apm_fft", FFT_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_IO, 1)) {
        AP_HAL::panic("Failed to start AP_GyroFFT update thread");
        return false;
    }

but like I said this may not be necessary as you can probably massively reduce the complexity of the implementation without this.

Happy for you to propose different semantics for VTX_OPTIONS or create a separate VTX_PITMODE flag if that makes sense. What is the workflow with "in-range" and "out-range"?

[andyp1per]

You should set the priority to IO_PRIORITY

My doubts about using a thread are that this is only applicable when not armed, but the thread will be active (and most importantly the stack space) when flying.

[wapophis]

My doubts about using a thread are that this is only applicable when not armed, but the thread will be active (and most importantly the stack space) when flying.

That's an important point to be considered, because it will be usefull to setup the vtx power when flying in order to optimize energy waste. Anyway that feature could be discused at future.
In ardupiltot docs there is no info about the behaviour you refer :O

[wapophis]

This is the updated status if the overall changes.
About the stack size, are there anyway yo check It?

• There is no need for the frontend/backend split, this makes the code unnecessarily complex and large - it should be possible to consolidate into a much smaller set of files. Flash usage is a priority for us and at the moment I suspect the flash usage of this library will be large.
  Actualized status: Removed

• You should remove AP_Vtx_Backend and use libraries/AP_RCTelemetry/AP_VideoTX - there is a lot of duplicated code that can be removed in favour of the existing code (or modify the existing code)
  Actualized status: Partially integrated.

• The betaflight C code for smartaudio_protocol needs to be converted into C++ and merged into the AP_SmartAudio implementation

Actualized status: need your support. Any example plz?

• The update loop and config should be put in libraries/AP_Vehicle rather than Copter. AP_VideoTX already has an update look so I suspect that this could just call into the SA code.

Actualized status: fixed

• Any extra config should be incorporated into AP_VideoTX and there rest re-used from AP_VideoTX

Actualized status: in progress

• You should use a define in the smart audio library to enable/disable rather than putting in Copter - have a look at libraries/AP_Camera/AP_RunCam.h for an example
  Actualized status: fixed

• If I read the code correctly it is attempting to be a state machine - I think this probably makes the code more complicated than it needs to be and could be removed in favour of a thread. betaflight does not have support for threads which is why they have done it this way - but a state machine is also possible and may be ok once some of the other complexity has been removed.

Actualized status: refactored

• You should remove all the whitespace changes from files unrelated to this PR.

Actualized status: pending

[andyp1per]

• So for example libraries/AP_BLHeli/ is code that has come from betaflight and been converted
• You can get the stack size by ftp'ing @SYS/threads.txt from mavproxy
• Conversion to use AP_VideoTX - I am going to work on Tramp support, I can probably give an example of this in the process

[wapophis]

• Conversion to use AP_VideoTX - I am going to work on Tramp support, I can probably give an example of this in the process

Integration with AP_VideoTX is almost complete, i'm writing the values directly to the singleton when call get_readings method is called, before i fetch the internal SA values from the buffer, and make any transformación needed in the same method. Because argument to get_readings is a pointer it's support múltiple vtx configuration if needed

[andyp1per]

@wapophis great! Just make sure that your code copes with something else writing these values - so for instance we have Spektrum VTX support which will allow these values to be written by the TX in which case your driver should be able to detect the change and send the appropriate SA commands

[wapophis]

@andyp1per good morning, thats the sequence i'm thinking to process external updates from AP_VideoTX, what's your opinion?

Keep polling updates on AP_VideoTX seems not good, maybe using #ifdef pointing to spec enabled pej: #ifdef SMARTAUDIO , could trigguers sync tasks from AP_VideoTx to the enabled specs implemented ? As far i can see, if there is desync between the smartaudio because an error it going to be needed to implement sempahores in vtx updates methods to prevent the fight between the third_party access and the smartaudio access. Sure there are other derivates that i'm not having in account yet.
Hope to start a discussion about which way the AP_VideoTX interaction you keep in mind.

Final approach is:

[andyp1per]

@wapophis I think this is mostly right. You can avoid polling updates through the cached copies held by AP_VideoTX - this is what CRSF does, it calls have_params_changed() to decide whether an update is necessary and then loops through the settings calling for instance update_power() which returns true if the power needs updating.

[wapophis]

@andyp1per questions about the booting process. When vtx boots, we can query hw-vtx to get his settings, then we can store the settings into ap_vtx, or get the settings from the ap_vtx and update the hw-vtx settings to set the values defined by the user.
What do you think about setting a param (FIRST_BOOT for example) which models this behaviour ? :

• Enabled: HW-VTX values are loaded into params.
• Disabled: AP-VTX values are setted into HW-VTX.

Once the user make the first-boot process (which can enabled by user anytime) , the changes flow is AP-VTX -> HW-VTX.

[andyp1per]

@wapophis I'm not sure its worth it as its a first time configuration issue. We already have this issue with configuring the UART and making sure the VTX is speaking the right protocol. Better to just doc in the wiki I think rather than adding a new parameter.

[andyp1per]

@wapophis doesn't look squashed, you still have 166 commits - you need to squash these into 1 commit per module, each commit message starting with the module name, e.g. AP_SmartAudio: do stuff
Here's a squash tutorial: https://medium.com/@slamflipstrom/a-beginners-guide-to-squashing-commits-with-git-rebase-8185cf6e62ec
You also need to rebase the PR as several files have changed since you modified them,

[andyp1per]

I pulled your branch and built for a Durandal - the code does not build (I had to remove crc8_dvb_s2_update) and when I had it built and loaded I was unable to connect via mission planner - so something is messed up in the serial handling with this change. You'll need to at least get it to a state that you can get some access to the board.

[wapophis]

This is the problem which is not failing using sitl but fails on real fc.

[andyp1per]

@wapophis I'll try it thanks - try and get the CI builds above clean.

[andyp1per]

Here are a couple of examples:

ardupilot/libraries/AP_RCProtocol/AP_RCProtocol.cpp

Line 258 in 11b3c62

uint32_t n = added.uart->available();

ardupilot/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp

Line 145 in 11b3c62

if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > CRSF_MAX_FRAME_TIME_US) {

Notice how the timeout / bytes available / incomplete message are incorporated into the loop

[wapophis]

@andyp1per the splitted reading from response buffer are now working. I'd made some test in 4 bytes packets and after some little rework on the read_response is working now. (fixed on db8526f)

I'd detected a problem with the timings in the main loop, which makes laggy the syncing changes with vtx, now it's fixed too, performing well in one loop usually. (fixed on 79ec8d4)

Finally the Ap_SmartAudio, is updating the AP_videoTx values when the hot_deploy param is enabled, syncing params persisted in the eprom with the "_current_" style backend params. making the syncing to work without needing to reboot de FC. ( pushed in e8a6300)

Other things to reply:

• PushForce: The strategy is to discard changes that were repeated. For example: if user is making channel selection fast, the vtx is only interested in the final one, using this approach retains the last five changes in queue. I can add more logic to the input into the queue, discarding events which are repeated and keeping the ones which are diferent type of commands. I think that this feature will be needed, but first i'll consolidate the protocol implementation without features which are not needed (like hot deploy which is basically for debugging)

[andyp1per]

With the latest I eventually get this, but it takes a long time - feels like something is not quite right

SA: send_request(): 0xAA 0x55 0x03 0x00 0x9F
SA: Ap_SmartAudio: byte discard:00
SA: read_response(): 0x55
SA: INPUT FILTERED FROM UART IS SHORTER THAN EXPLICIT RESPONSE LENGTH
SA: request_settings()
SA: send_request(): 0xAA 0x55 0x03 0x00 0x9F
SA: read_response(): 0xAA 0x55 0x11 0x0D 0x19 0x00 0x10 0x16 0xF8 0x0E 0x04 0x00 0x0E 0x14 0x1A 0x1D 0xFD 0x00
SA:                                                     selecting update path using: 0APM: AP_SmartAudio: Operation REQUEST SETTINGS VERSION 2.1 completed since 102 milliseconds
F
SA:                   parse_frame_response:: freq update from band and chan settings
SA: UPDATE AP_VTX->HW_VTX: CHAN
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x07 0x01 0x22 0x63
APM: RESPONSE RETRY:0
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA: send_request(): 0xAA 0x55 0x0B 0x01 0x00 0x2D
SA:
AP_SmartAudio: Printing Status debug info [58280,3067]
SA: AP_SmartAudio: Hardware vtx is initialized, has no pending updates and is waiting response with hot deploy ON
SA: AP_SmartAudio STATS ERRORS [Unknow Responses Types count: 0, Bad lenght Responses count: 0,Bad Headers Responses count: 0, Bad Crc response count: 0, Unparseable Responses count: 414 ]
SA: AP_SmartAudio STATS AUTO ADJUST BAUD STATS [Responses count: 1, Request counter: 9, Baud Rate: 4980, Baud Rate direcction:Down ]
SA: AP_SmartAudio STATS COMMANDS  [ FAILED READS: 414, REQ_SETTINGS: 46, OTHER REQUESTS:3 ]
SA: UPDATE AP_VTX->HW_VTX: CHAN
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x07 0x01 0x22 0x63
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA: send_request(): 0xAA 0x55 0x0B 0x01 0x00 0x2D

[wapophis]

Thanks Andy i take a look

[andyp1per]

I've spent a fair bit of time on this today. Fixed a few bugs and restructured a few things that were wrong - not quite working correctly, but closer I think. Will put up a PR shortly.

[wapophis]

Fine, thanks. This night i'll check it

[andyp1per]

This is the PR wapophis#4

[andyp1per]

With my change I now get this:

SA: send_request(): 0xAA 0x55 0x03 0x00 0x9F
SA: read_response(): 0x1A 0x1D 0x1C
SA: selecting update path using: 0F
SA:                   parse_frame_response:: freq update from band and chan settings
SA: UPDATE AP_VTX->HW_VTX: Ch: 2, Band: 4
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x07 0x01 0x22 0x63
APM: AP_SmartAudio: Operation REQUEST SETTINGS VERSION 2.1 completed since 160 milliseconds
APM: SmartAudio initialized, SA 2.1, Band 3, Ch 1
APM: RESPONSE RETRY:171
SA: UPDATE AP_VTX->HW_VTX: Ch: 2, Band: 4
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA: UPDATE AP_VTX->HW_VTX: Ch: 2, Band: 4
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA: UPDATE AP_VTX->HW_VTX: Ch: 2, Band: 4
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA: UPDATE AP_VTX->HW_VTX: Ch: 2, Band: 4
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA: UPDATE AP_VTX->HW_VTX: Ch: 2, Band: 4
SA: AP_SmartAudio: Setting channel to 34
SA: UPDATE AP_VTX->HW_VTX: OPTIONS
SA: set_operation_mode(): Changing operation mode to 00
SA: send_request(): 0xAA 0x55 0x05 0x01 0x0E 0xC3
SA:
AP_SmartAudio: Printing Status debug info [91893,3003]

Fairly continuously after it finally communicates. The device also seems to switch into 800mw mode - which is not what is configured, so something not correct as yet.

[wapophis]

I'm checking the pr, one thing that it's seems strange is that the first reading from response buffer seems partial and i dont see the complete response from vtx, and subsequent requests has not been responsed. Other thing i notice is that the pulldown for the requests has been removed. Anyway I'll try with this pr into my FC with sma 2.0 and check if it's already working.

[andyp1per]

I think you should put the pulldown back in - I just took it out so I could see the trace properly on the oscilloscope

[andyp1per]

There are still some timing things to work out. I suspect the low priority of the thread is contributing to this.

[andyp1per]

There is definitely something wrong with the pitmode and power settings. I think I have a fix for the pitmode stuff

[andyp1per]

Part of the issue is I have totally toasted the lipo I was testing with, will find another ...

[andyp1per]

Ok, you cannot lock/unlock the VTX via SA it seems - at least not on my Evo, you can only query the setting.

[andyp1per]

@wapophis I have updated my PR. There are still a fair few bugs in the logic handling of this PR:

• AP_VideoTX already knows the power level in dbm, we should use that instead of re-calculating. I have a fixed a couple of instances of this but it needs a bit more cleanup - in particular the logic for SA 2.1 was wrong
• The level 0 byte definitely confuses my Evo and it seems to work perfectly well without it (much better in fact)
• The SA autobaud needs to quickly cycle through the baud rates until it finds something that works and then stay there until a response is missed. 10 ticks is too much I think - you could probably change the baud every 100ms and it would be fine
• The syncing between AP_VideoTX and SA is not right and I think could be cleaned up a fair bit - it is still trying to set settings that do not need to be changed
• Please, please run the code through the AP formatter : https://ardupilot.org/dev/docs/style-guide.html and adhere to this, it makes it much easier to review
• Also please note that my switch of the SA structures to C++ style revealed a fair few initialization bugs - so wherever possible we need to stick to C++ style

I'll do a more detailed review now that it's roughly working for me, out of time for actual testing for now I am afraid

[andyp1per]

Hey @wapophis what progress here?

[wapophis]

Hey @wapophis what progress here?

Yes Andy because of working presure i had not too much time to dev. Next weekend i'll push to the pr. During this dev impass i have been flying whithout any collateral impact.

[andyp1per]

Hi @wapophis any movement on this? I'll clone your PR and work on it if you are not going to get to it

[skorokithakis]

This would be fantastic to merge, currently SmartAudio support is the only thing keeping me from using AP, as the VTX can get very hot if it's always set to the highest power, especially testing on the bench.

[andyp1per]

Ok, I have started work on this

[andyp1per]

This has been subsumed by #16464 which is now merged