Loiter_turns Command Troubleshooting

The problem we faced at today's flight test was:

• The LOITER_TURNS command recognised the centre of the circle, BUT it seemed to rotate around the point with no radius
• However, the CIRCLE_RADIUS is correctly set, and it is working in CIRCLE mode
• After it completed LOITER_TURNS, and heading to the next waypoint, the altitude drift to ~33m, far away from the designated 20m height.

The first two bullets indicate that the problem is something to do with the code. AND IT IS!

Here is the code of our concern:

---------------------------------------------------------------------------------------

case MAV_CMD_NAV_LOITER_TURNS:                      // MAV ID: 18
          copy_location = true;
          num_turns = packet.param1;                      // number of times to circle is held in param1
          radius_m = fabs(packet.param3);                 // radius in meters is held in high in param3
          cmd.p1 = (((uint16_t)radius_m)<<8) | (uint16_t)num_turns;   // store radius in high byte of p1, num turns in low byte of p1
          cmd.content.location.flags.loiter_ccw = (packet.param3 < 0);
          break;

---------------------------------------------------------------------------------------

This clearly shows that the third parameter of the LOITER_TURNS command is used to store the radius data. HENCE, we shall not simply put 1 or -1 to indicate direction of turn, but should type in something like +20 or -15.


There is a minor issue hiding in the code above. The cmd structure in the code only accept one general parameter. This means that the num_of_turn and radius_m must be squeezed in to the single uint16_t variable. This is evident below:

-------------------------------------------------------------------------------------

struct PACKED Mission_Command {
        uint16_t index;             // this commands position in the command list
        uint8_t id;                 // mavlink command id
        // CHM - the commande structure have only 1 parameter in short int. This cause the loiter_turn cmd to be squeezed (num of turn + turn_radius).
        uint16_t p1;                // general purpose parameter 1
        Content content;
    };

-------------------------------------------------------------------------------------

In conclusion, the 1 should be changed to larger number. TO BE VERIFIED BY TOMMOROW