Referencing this code from the mount control library for storm32 gimbals, the pitch is reversed prior to being sent to the gimbal.
https://github.com/ArduPilot/ardupilot/blob/c808ee2f497e7a7a6d570632a3224e655b377323/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp#L172
Can anyone explain why this is done? @olliw42?
STorM32 uses a different coordinate frame for the attitude than ArduPilot
(thatās because it is a gimbal controller and not a flight controller)(and because at the time then I started STorM32 I didnāt had any clue why aircrafts use the frame they use, and so chose the one I found most convenient; in hindsight not my smartest move, today I would do differently, but we all are on a learning curve)
This results in angles which differ in sign on the pitch axis and the yaw axis (check the code, both are reversed). This is actually not totally correct mathematically. What one actually would have to do is to convert to (or use the) quaternion/DCM, rotate the quaternion/DCM to account for the different frame, and then to recalculate the angles. However, thatās a bit expensive and the -Ā± approach appears to work sufficiently well in practice (since roll usually isnāt that large).
There is a slight pitfall: This reversal is need when one calls ānativeā STorM32 functions, but not when one calls Mavlink commands. In the latter case this reversal is done by STorM32 in order to be compatible.
Hope that helps.
Thanks @olliw42
I think this is leading to the problem folks have been having with the Solo. Or at least one person (@jubalr). That automated reversing wasnāt working. So in RC targeting, it was working properly. But in mavlink targeting (as is used during smart shots), the pitch was running backwards. Is there a setting in the storm32 controls that could be set incorrectly causing that to happen? Or a setting in ArduCopter that is messing with it? I donāt own a storm32 gimbal at this time, so Iām not familiar with all its settings.
Iām not sure I really understand the issue, since it should be handled within the AP_Mount library and not depend on rc_targeting or mavlink-targeting, I think in either mode eventually the same serial/Mavlink command is send
the serial mount library is IMHO not very well structured, so it might be that the reversal isnāt done correctly in all modes. I also wouldnāt rule out a bug in STorM32 which you hit, I never would, but would not consider this the first guess.
Iām not sure if it could be a STorM32 setting. It is possible to reverse the response to rc signals, but I think the AP_Mount uses the absolute angle commands there this should not matter. I would need to loo into the code of the AP_Mount again.
Do you see that issue for both the Mavlink STorM32 mount and the and the serial STorM32 mount?
Btw, you can find a sort of reference implementation in BetaCopter, or since recently in this PR https://github.com/ArduPilot/ardupilot/pull/7167. It could be useful if you could test BetaCopter and check if it shows the same problem. This might tell if its AP_Mount related or STorM32 related.
quickly looked at the code you linked too
I think it may indeed handle the reversal inconsistently
e.g., it reads in the angles from the STorM32 and stores them in current_angles, which are send out by the mavlink mount status without any reversal, so I would think that the gimbal angles are reported incorrectly (you should be able to test that easily with MissionPlanner)
so could be that also rc targeting is treated incorrectly
it could be important to know which STorM32 firmware version you are referring to
Another user just today got the Storm32 up and running on his Solo, and the pitch is working properly in both RC targeting and mavlink targeting modes. He did a cable cam smart shot, and the gimbal performed beautifully. This was on an otherwise stock solo, which uses fork of ArduCopter 3.3-RC2 I think. Not sure if anything changed between then and 3.5.
They both found that if you have the Storm32 emit hearbeat packets, everything goes nuts. If you turn off the storm32 heartbeat, it works. Is that heartbeat critical to something.
well, this depends on which Mount they had selected. If it is serial (i.e. the one you linked to), which appears to be the case, then I think itās clear that it would go nuts with the Mavlink heartbeat enabled. However, why would one do that, it should be clear that the two available STorM32 mounts are exclusive.
I looked further into the codes.
For the serial mount, RC targeting IMHO calls the same send-angles command as Mavlink targeting, so there shouldnāt be a difference. There is however, IMHO, the bug that the angles reported back in the status message are not correct, i.e. do not have the pitch&yaw reversed. I think you want to drop a PR or issue to get this rectified. Itās IMHO a clear bug (as said I think you can test that easily, maybe you want to do that to confirm/disconfirm).
I also looked at the STorM32 code, and realized that I seem to have a bug myself in the handling of the MAV_CMD_DO_MOUNT_CONTROL message, i.e., I donāt do the reversal here (in contrast to my claim in the above). So, I think you should find that then you use the Mavlink STorM32 mount that it will behave incorrectly, in that RC targeting and Mavlink targeting have opposite signs, similar to what you describe in the first post.
Maybe you have a chance to test that and confirm/disconfirm that.
BTW, fantastic to hear that the STorM32 works well with Soloās smart shoots. Really great. I would love to see some footage posted, e.g., in the rcgroups thread! (you know, seeing users getting results is the reward in that game).
cheers, Olli
Which library is the mavlink storm32? I only see storm32 and storm32 serial.
http://www.olliw.eu/storm32bgc-wiki/Using_STorM32_with_ArduPilot#STorM32_-_ArduPilot_Support
or
http://ardupilot.org/copter/docs/common-storm32-gimbal.html#common-storm32-gimbal|
MNT_TYPE = 4 -> Mavlink -> AP_MOUNT_SToRM32
MNT_TYPE = 5 -> Serial -> AP_MOUNT_SToRM32_serial
So we still have some issues now that Iāve gotten more details from some users.
With MNT_TYPE = 4 (Storm32 Mavlink), and the gimbalās mavlink heartbeat enabled, everything goes nuts. The autopilot gets all kinds of position errors, calibration errors, etc. Itās like the gimbalās attitude data is screwing up the autopilotās attitude data. If you change the gimbalās mavlink component ID to something other than the default of 67, the attitude malfunctions go away, but the gimbal is unresponsive to controls.
With MNT_TYPE = 4 (Storm32 Mavlink) but the gimbalās heartbeat disabled, it only has this attitude malfunction about 20% of the time. This doesnāt seem like a proper configuration since ArduCopter is set for mavlink, but I think this setting in Storm32 disables mavlink?
With MNT_TYPE = 5 (storm32 serial) and storm32 heartbeat disabled, RC Targeting mode works, but mavlink targeting mode does not.
Interesting reading. Sounds like we may be onto something
this doesnāt make any sense to me
first, I think we MUST know for which combination of ArduCopter firmware version and STorM32 firmware version a certain observation is observed. Otherwise discussing a observation is useless.
I say this because I know for a fact that there had been combinations of ArduCopter and STorM32 for which the behavior was not at all like you describe, and in fact were working (except maybe that some signs were not correct). Sadly I canāt say myself which combination(s) this had been, since thatās quite some time ago when I did that testings. I also might not have tested the serial well, so I may speak only for MNT_TYPE 4, but I do speak for at least one MNT_TYPE.
I thus do not think that the above is a representative reflection, but either is maybe a Solo issue, or something important has changed in ArduPilot in the last year (I remember e.g. that a year, or was it 1 1/2 years, ago, with Mavlink2 coming, all sorts of problems occurred because the Mavlink2 implementation wasnāt fletched out, donāt know what has happened since then and if that ever was weeded out, Francisco/Oxinarf and Luis/Lvale had been involved in that discussion at the time)(I also remember that about the same time - at the request of ArduPilot devs - I made the recognition of Mavlink messages more flexible, conforming to the āstandardā).
Your bullet 2 I would think is a smoking gun demonstration of a bug (or several) in the ArduPilot firmware ā¦ with STorM32 heartbeats disabled it really doesnāt do much. You probably could disconnect the STorM32 and would see the very same malfunctions.
Also bullet 1 should make you wary.
Btw, you know that you can disable the emission of the STorM32 gimbal attitude data, which would allow you to test your speculation in bullet 1.
Again, you maybe want to try betacopter and see how it does.
I think one also should settle on one MNT_TYPE and concentrate on it first.
My 2 cents. 
Which setting should they disable to stop the attitude data? Also, is leaving the mavlink system and component IDs default correct?
the parameter āMavlink Configurationā can be set to āno heartbeatā, āemit heartbeatā, āheartbeat + attitudeā. Set it to āemit heartbeatā.
I think that the default system and component ID are correct. It might in fact be that they have to be set to the default values, there might be some implicit dependencies in ArduPilot/MissionPlanner/ā¦ Iām not sure.
Generally, itās a long while ago that Iāve played with ArduPilotās STorM32 mounts, so answers to related questions, such as the previous paragraph, may or may not be correct. I think that e.g. Luis/lvale knows everything about it. Maybe you want to get his opinion too.
On an off topic question, does anyone know where you can get the NT Storm with the motor controller /Encoders pre-wired?
so, I was curious now and did some bench testing, using a pixracer and a STorM32, and MissionPlanner. The firmware in the pixracer was ArduCopter 3.6 dev (so not Solo!).
many things work as expected, but not all, Iām not sure itās correct or wrong though
first, I have to say that in some recent firmware version a bug has crept in, which makes the serial hang up as soon as the mavlink heartbeat is enabled. Will be corrected in the next version, and should not be present in somewhat older versions.
I connected the STorM32 to serial3, which Iāve set to 115, protocol 1. I did when basically 5 test, where in each test I tried the available settings for the parameter āMavlink Configurationā, namely
"no heartbeat"
āemit heartbeatā (called hb in short)
āheartbeat+attitudeā (called hb+a in short)
āh.b.+mountstatusā (called hb+ms in short)
1. STorM32 directly connected via a USB-TTL adapter to MissionPlanner, MNT_TPYE = 0
all three options hb, hb+a, hb+ms work as expected, that is:
2. STorM32 connected to pixracer, pixracer to MP, MNT_TPYE = 0, Mavlink ID = 1, 67
all three options hb, hb+a, hb+ms work as expected, that is:
3. STorM32 connected to pixracer, pixracer to MP, MNT_TPYE = 0, Mavlink ID = 71, 67
all three options hb, hb+a, hb+ms work as expected, that is:
These tests I did to see if the STorM32ās Mavlink communication is basically correct
4. STorM32 connected to pixracer, pixracer to MP, MNT_TPYE = 4, Mavlink ID = 1, 67
once the pixracer has booted I can see a lot traffic coming from the STorM32, so the MNT_TYPE 4 must make it send a lot (no problem by itself)
things work nearly as expected
the latter I find unexpected. I would have thought that when a mount is a component, that then its mount_status message would be recognized by QUADROTOR-1. The MNT_TYPE 4 does not emit a mount_status message to the QUADROTOR-1, so this would have appeared as the logical expectation.
EDIT: I note that 1 to 1 1/2 years ago this was working as expected, and was the reason for the option hb+ms. This allowed e.g. to see the MPās gimbal point in the map !!
I probably have misinterpreted this, and this is the intended behavior for a Mavlink mount.
5. STorM32 connected to pixracer, pixracer to MP, MNT_TPYE = 4, Mavlink ID = 71, 67
once the pixracer has booted I can see a lot traffic coming from the STorM32, so the MNT_TYPE 4 must make it send a lot (no problem by itself)
things do NOT go well here:
not sure if that is the correct behavior.
fin
You may want to try to reproduce that with a solo
I would conclude that the best bet is to use ID = 1,67 (and not 71,67). Since no mount_status is send to the flight controller, itās pointless to set anything higher than hb.
I finally note: The info in the wiki, http://ardupilot.org/copter/docs/common-storm32-gimbal.html, and in here, http://ardupilot.org/dev/docs/code-overview-adding-support-for-a-new-mavlink-gimbal.html, differ in the Mavlink system ID. It seems that in this regard indeed something has changed in ArduPilot some time ago.
one note I forgot:
itās also a while ago, but I believe to remember that in the code I found a mechanism which can block/modify the Mavlink communication in some ways - there is a related variable. I also believe to remember that I found it to be used by the Solo mount. At least I have a marker in my brain which says āOh, they had to play dirty tricks with the Mavlink for the Solo gimbalā.
It might be worthwhile to track that down, as it could indicate that there is a fundamental difference between how Mavlink works for a Solo gimbal, and any other gimbal.
hope all that help you guys to better track down your issues
so, next step, Iāve analyzed in more depth the code for the STorM32 Mavlink mount (AP_Mount_SToRM32.cpp/h).
First, I looked up how the STorM32 handles messages, depending on the target systems and component IDs (as relevant for the MOUNT_CONTROL msg).
EDIT: Note, that these rules hold for the latest firmwares, not for v0.96! The rules for v0.96 are given in post 50.
I firmly believe thatās the correct behavior.
Then I looked up how the gimbal system ID should be set, that is if it can be āanyā value such as the default system = 71, or should be set to the ArduPilotās system = 1. As mentioned before, there is a conflict in the wiki. From the code, in agreement with the wiki entry on Mavlink Gimbals, it should be set to system = 1. With system = 71, the RC_TARGETING and GPS_POINT modes would work, but not the MAVLINK_TARGETING mode.
Conclusion, set STorM32 System ID = 1.
The crucial point to note is that in the RC_TARGETING/GPS_POINT modes and the MAVLINK_TARGETING mode the MOUNT_CONTROL msg is fundamentally differently treated. In the former two cases a MOUNT_CONTROL msg with target system = STorM32 system ID and target component = STorM32 component ID is send to the gimbal, irrespective of the values of the IDs. However, in the MAVLINK_TARGETING mode the msg from e.g. a GCS must be both processed locally and routed to the component.
So, I looked up the processing and routing rules (https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/MAVLink_routing.cpp or http://ardupilot.org/dev/docs/mavlink-routing-in-ardupilot.html). I considered cases that ArduPilot receives a MOUNT_CONTROL msg target system/components of {0,arbitrary}, {1,0}, and {1,gimbal component}. I noted that the rules do NOT cover the case {1,0}. So I checked with the code and from
https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/MAVLink_routing.cpp#L126
it is clear that no target component and target component = 0 are treated equivalently, as expected by common sense.
Conclusion, the descriptions in https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/MAVLink_routing.cpp or http://ardupilot.org/dev/docs/mavlink-routing-in-ardupilot.html are incomplete, and should be adapted.
The msges with {0,arbitrary} and {1,0} would be locally processed and routed to the gimbal, as it should be. The msg with {1,gimbal component} would NOT be processed locally but routed, which means that here MAVLINK_TARGETING would not work. This is probably the intended behavior, but Iām not convinced itās the best intention. I donāt know with what target the Solo smart shot is sending the MOUNT_CONTROL msg to the gimbal, you guys may want to check this, but as much as I can tell this in general looks ok to me.
Iād like to recall that even when the STorM32ās heartbeat is disabled, some data bytes are send to it. From the processing and routing rules this should not happen.
Conclusion, there is something up in the ArduPilot code. Not critical, but.
Lastly, I looked very carefully at the -Ā± sign handling. And as mentioned before, there is indeed a bug in the STorM32 code: For the MOUNT_CONTROL msg it does NOT do the sign changes internally, as it is supposed to do. This bug must have been there since a long, long time but showed up only know. In the AP_Mount_SToRM32 code this bug was handled for the RC_TARGETING/GPS_POINT modes by explicitly doing the sign change, but for the MAVLINK_TARGETING mode this didnāt work (since in this case the MOUNT_CONTROL msg is not handled by the mount).
Conclusion, there is indeed a bug in the STorM32 code with the result that in the MAVLINK_TARGETING mode pitch (and yaw) is treated with a wrong sign. THX Pedals2Paddles !!!
ToDo:
In the next STorM32 firmware version this bug will be corrected. At the same time, these three lines should be deleted:
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Mount/AP_Mount_SToRM32.cpp#L138-L140
Maybe anyone would want to place a corresponding PR (I canāt do PRs).
The disadvantage of this (correct) solution is that users of the old I2C-based STorM32, which is limited to the old firmware v0.96, would find their gimbal to not behave correctly anymore.
Thx for pointing me to this bug!
I would be glad to hear if that resolves the issues you have seen with Solo&STorM32.
Btw: For the STorM32 serial mount this reversed-pitch issue should not be present. So, you guys may test/use this. As mentioned before it has the bug that the MOUNT_STATUS message has uncorrected angles. One could correct this by placing the two ā-ā sign into the 1st and 3rd lines here
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp#L273-L275
Cheers, Olli