Gimbal API redesign (support more than one gimbal, support streams an…

…d commands) (#351)

* gimbal: improve API

This aims at improving the API by:
- Adding an argument for send mode (either command or message stream)
- Includes the mode into the setter because that's when it's sent.
  Otherwise, the set_mode call itself doesn't have any effect which is
  not very intuitive.

* gimbal: extend to multiple gimbals

This will allow multiple gimbals to be controlled from just the one
gimbal plugin instance. The goal is not to have multiple instances like
it is (currently) done for cameras.

* gimbal: model, not product

protos/gimbal/gimbal.proto

@@ -9,41 +9,29 @@ option java_outer_classname = "GimbalProto";
 
 // Provide control over a gimbal.
 service GimbalService {
+
     /*
-     *
      * Set gimbal roll, pitch and yaw angles.
      *
      * This sets the desired roll, pitch and yaw angles of a gimbal.
      * Will return when the command is accepted, however, it might
      * take the gimbal longer to actually be set to the new angles.
-     */
-    rpc SetAngles(SetAnglesRequest) returns(SetAnglesResponse) {}
-    /*
-     *
-     * Set gimbal pitch and yaw angles.
      *
-     * This sets the desired pitch and yaw angles of a gimbal.
-     * Will return when the command is accepted, however, it might
-     * take the gimbal longer to actually be set to the new angles.
+     * Note that the roll angle needs to be set to 0 when send_mode is Once.
      */
-    rpc SetPitchAndYaw(SetPitchAndYawRequest) returns(SetPitchAndYawResponse) {}
+    rpc SetAngles(SetAnglesRequest) returns(SetAnglesResponse) {}
+
     /*
+     * Set gimbal angular rates.
      *
-     * Set gimbal angular rates around pitch and yaw axes.
-     *
-     * This sets the desired angular rates around pitch and yaw axes of a gimbal.
+     * This sets the desired angular rates around roll, pitch and yaw axes of a gimbal.
      * Will return when the command is accepted, however, it might
      * take the gimbal longer to actually reach the angular rate.
-     */
-    rpc SetPitchRateAndYawRate(SetPitchRateAndYawRateRequest) returns(SetPitchRateAndYawRateResponse) {}
-    /*
-     * Set gimbal mode.
      *
-     * This sets the desired yaw mode of a gimbal.
-     * Will return when the command is accepted. However, it might
-     * take the gimbal longer to actually be set to the new angles.
+     * Note that the roll angle needs to be set to 0 when send_mode is Once.
      */
-    rpc SetMode(SetModeRequest) returns(SetModeResponse) {}
+    rpc SetAngularRates(SetAngularRatesRequest) returns(SetAngularRatesResponse) {}
+
     /*
      * Set gimbal region of interest (ROI).
      *
@@ -54,6 +42,7 @@ service GimbalService {
      * take the gimbal longer to actually rotate to the ROI.
      */
      rpc SetRoiLocation(SetRoiLocationRequest) returns(SetRoiLocationResponse) {}
+
      /*
       * Take control.
       *
@@ -66,86 +55,111 @@ service GimbalService {
       * override each other and should therefore do it carefully.
       */
      rpc TakeControl(TakeControlRequest) returns(TakeControlResponse) {}
+
      /*
       * Release control.
       *
       * Release control, such that other components can control the gimbal.
       */
      rpc ReleaseControl(ReleaseControlRequest) returns(ReleaseControlResponse) {}
+
+     /*
+      * Subscribe to list of gimbals.
+      *
+      * This allows to find out what gimbals are connected to the system.
+      * Based on the gimbal ID, we can then address a specific gimbal.
+      */
+     rpc SubscribeGimbalList(SubscribeGimbalListRequest) returns(stream GimbalListResponse) {}
+
      /*
       * Subscribe to control status updates.
       *
       * This allows a component to know if it has primary, secondary or
       * no control over the gimbal. Also, it gives the system and component ids
       * of the other components in control (if any).
       */
-     rpc SubscribeControl(SubscribeControlRequest) returns(stream ControlResponse) {}
+     rpc SubscribeControlStatus(SubscribeControlStatusRequest) returns(stream ControlStatusResponse) { option (mavsdk.options.async_type) = ASYNC; }
+
+     /*
+      * Get control status for specific gimbal.
+      */
+     rpc GetControlStatus(GetControlStatusRequest) returns (GetControlStatusResponse) { option (mavsdk.options.async_type) = SYNC; }
+
      /*
       * Subscribe to attitude updates.
       *
       * This gets you the gimbal's attitude and angular rate.
       */
-     rpc SubscribeAttitude(SubscribeAttitudeRequest) returns(stream AttitudeResponse) {}
+     rpc SubscribeAttitude(SubscribeAttitudeRequest) returns(stream AttitudeResponse) { option (mavsdk.options.async_type) = ASYNC; }
+
+     /*
+      * Get attitude for specific gimbal.
+      */
+     rpc GetAttitude(GetAttitudeRequest) returns (GetAttitudeResponse) { option (mavsdk.options.async_type) = SYNC; }
 }
 
 message SetAnglesRequest {
-    float roll_deg = 1; // Roll angle in degrees
-    float pitch_deg = 2; // Pitch angle in degrees (negative points down)
-    float yaw_deg = 3; // Yaw angle in degrees (positive is clock-wise, range: -180 to 180 or 0 to 360)
+    int32 gimbal_id = 1; // Gimbal id to address (0 for all gimbals)
+    float roll_deg = 2; // Roll angle in degrees (negative down on the right)
+    float pitch_deg = 3; // Pitch angle in degrees (negative points down)
+    float yaw_deg = 4; // Yaw angle in degrees (positive is clock-wise, range: -180 to 180 or 0 to 360)
+    GimbalMode gimbal_mode = 5; // Gimbal mode to use
+    SendMode send_mode = 6; // Send mode of command/setpoint
 }
 message SetAnglesResponse {
     GimbalResult gimbal_result = 1;
 }
 
-message SetPitchAndYawRequest {
-    float pitch_deg = 1; // Pitch angle in degrees (negative points down)
-    float yaw_deg = 2; // Yaw angle in degrees (positive is clock-wise, range: -180 to 180 or 0 to 360)
-}
-message SetPitchAndYawResponse {
-    GimbalResult gimbal_result = 1;
-}
-
-message SetPitchRateAndYawRateRequest {
-    float pitch_rate_deg_s = 1; // Angular rate around pitch axis in degrees/second (negative downward)
-    float yaw_rate_deg_s = 2; // Angular rate around yaw axis in degrees/second (positive is clock-wise)
-}
-message SetPitchRateAndYawRateResponse {
-    GimbalResult gimbal_result = 1;
-}
-
-message SetModeRequest {
-    GimbalMode gimbal_mode = 1; // The mode to be set.
+message SetAngularRatesRequest {
+    int32 gimbal_id = 1; // Gimbal id to address (0 for all gimbals)
+    float roll_rate_deg_s = 2; // Angular rate around roll axis in degrees/second (negative down on the right)
+    float pitch_rate_deg_s = 3; // Angular rate around pitch axis in degrees/second (negative downward)
+    float yaw_rate_deg_s = 4; // Angular rate around yaw axis in degrees/second (positive is clock-wise)
+    GimbalMode gimbal_mode = 5; // Gimbal mode to use
+    SendMode send_mode = 6; // Send mode of command/setpoint
 }
-message SetModeResponse {
+message SetAngularRatesResponse {
     GimbalResult gimbal_result = 1;
 }
 
 message SetRoiLocationRequest {
-    double latitude_deg = 1; // Latitude in degrees
-    double longitude_deg = 2; // Longitude in degrees
-    float altitude_m = 3; // Altitude in metres (AMSL)
+    int32 gimbal_id = 1; // Gimbal id to address (0 for all gimbals)
+    double latitude_deg = 2; // Latitude in degrees
+    double longitude_deg = 3; // Longitude in degrees
+    float altitude_m = 4; // Altitude in metres (AMSL)
 }
 message SetRoiLocationResponse {
     GimbalResult gimbal_result = 1;
 }
 
 message TakeControlRequest {
-    ControlMode control_mode = 1; // Control mode (primary or secondary)
+    int32 gimbal_id = 1; // Gimbal id to address (0 for all gimbals)
+    ControlMode control_mode = 2; // Control mode (primary or secondary)
 }
 message TakeControlResponse {
     GimbalResult gimbal_result = 1;
 }
 
-message ReleaseControlRequest {}
+message ReleaseControlRequest {
+    int32 gimbal_id = 1; // Gimbal id to address (0 for all gimbals)
+}
 message ReleaseControlResponse {
     GimbalResult gimbal_result = 1;
 }
 
-message SubscribeControlRequest {}
-message ControlResponse {
+message SubscribeControlStatusRequest {}
+message ControlStatusResponse {
     ControlStatus control_status = 1; // Control status
 }
 
+message GetControlStatusRequest {
+    int32 gimbal_id = 1; // Gimbal ID
+}
+message GetControlStatusResponse {
+    GimbalResult gimbal_result = 1;
+    ControlStatus control_status = 2; // Control status
+}
+
 /*
  * Quaternion type.
  *
@@ -187,19 +201,33 @@ message AngularVelocityBody {
 
 // Gimbal attitude type
 message Attitude {
-    EulerAngle euler_angle_forward = 1; // Euler angle relative to forward
-    Quaternion quaternion_forward = 2; // Quaternion relative to forward
-    EulerAngle euler_angle_north = 3; // Euler angle relative to North
-    Quaternion quaternion_north = 4; // Quaternion relative to North
-    AngularVelocityBody angular_velocity = 5; // The angular rate
-    uint64 timestamp_us = 6; // Timestamp in microseconds
+    int32 gimbal_id = 1; // Gimbal ID
+    EulerAngle euler_angle_forward = 2; // Euler angle relative to forward
+    Quaternion quaternion_forward = 3; // Quaternion relative to forward
+    EulerAngle euler_angle_north = 4; // Euler angle relative to North
+    Quaternion quaternion_north = 5; // Quaternion relative to North
+    AngularVelocityBody angular_velocity = 6; // The angular rate
+    uint64 timestamp_us = 7; // Timestamp in microseconds
 }
 
 message SubscribeAttitudeRequest {}
 message AttitudeResponse {
     Attitude attitude = 1; // The attitude
 }
 
+message GetAttitudeRequest {
+    int32 gimbal_id = 1; // Gimbal ID
+}
+message GetAttitudeResponse {
+    GimbalResult gimbal_result = 1;
+    Attitude attitude = 2; // The attitude
+}
+
+message SubscribeGimbalListRequest {}
+message GimbalListResponse {
+    GimbalList gimbal_list = 1; // Gimbal list
+}
+
 // Gimbal mode type.
 enum GimbalMode {
     GIMBAL_MODE_YAW_FOLLOW = 0; // Yaw follow will point the gimbal to the vehicle heading
@@ -213,13 +241,35 @@ enum ControlMode {
     CONTROL_MODE_SECONDARY = 2; // To take secondary control over the gimbal
 }
 
+enum SendMode {
+    SEND_MODE_ONCE = 0; // Send command exactly once with quality of service (use for sporadic commands slower than 1 Hz)
+    SEND_MODE_STREAM = 1; // Stream setpoint without quality of service (use for setpoints faster than 1 Hz).
+}
+
+// Gimbal list item
+message GimbalItem {
+    int32 gimbal_id = 1; // ID to address it, starting at 1 (0 means all gimbals)
+    string vendor_name = 2; // Vendor name
+    string model_name = 3; // Model name
+    string custom_name = 4; // Custom name name
+    int32 gimbal_manager_component_id = 5; // MAVLink component of gimbal manager, for debugging purposes
+    int32 gimbal_device_id = 6; // MAVLink component of gimbal device
+}
+
+// Gimbal list
+message GimbalList {
+    repeated GimbalItem gimbals = 1; // Gimbal item.
+}
+
+
 // Control status
 message ControlStatus {
-    ControlMode control_mode = 1; // Control mode (none, primary or secondary)
-    int32 sysid_primary_control = 2; // Sysid of the component that has primary control over the gimbal (0 if no one is in control)
-    int32 compid_primary_control = 3; // Compid of the component that has primary control over the gimbal (0 if no one is in control)
-    int32 sysid_secondary_control = 4; // Sysid of the component that has secondary control over the gimbal (0 if no one is in control)
-    int32 compid_secondary_control = 5; // Compid of the component that has secondary control over the gimbal (0 if no one is in control)
+    int32 gimbal_id = 1; // Gimbal ID
+    ControlMode control_mode = 2; // Control mode (none, primary or secondary)
+    int32 sysid_primary_control = 3; // Sysid of the component that has primary control over the gimbal (0 if no one is in control)
+    int32 compid_primary_control = 4; // Compid of the component that has primary control over the gimbal (0 if no one is in control)
+    int32 sysid_secondary_control = 5; // Sysid of the component that has secondary control over the gimbal (0 if no one is in control)
+    int32 compid_secondary_control = 6; // Compid of the component that has secondary control over the gimbal (0 if no one is in control)
 }
 
 // Result type.
@@ -232,6 +282,7 @@ message GimbalResult {
         RESULT_TIMEOUT = 3; // Command timed out
         RESULT_UNSUPPORTED = 4; // Functionality not supported
         RESULT_NO_SYSTEM = 5; // No system connected
+        RESULT_INVALID_ARGUMENT = 6; // Invalid argument
     }
 
     Result result = 1; // Result enum value