AP_Mount: validate modes before setting them

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -46,6 +46,26 @@ bool AP_Mount_Backend::has_pitch_control() const
     return (_params.pitch_angle_min < _params.pitch_angle_max);
 }
 
+bool AP_Mount_Backend::valid_mode(MAV_MOUNT_MODE mode) const
+{
+    switch (mode) {
+    case MAV_MOUNT_MODE_RETRACT...MAV_MOUNT_MODE_HOME_LOCATION:
+        return true;
+    case MAV_MOUNT_MODE_ENUM_END:
+        return false;
+    }
+    return false;
+}
+
+bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
+{
+    if (!valid_mode(mode)) {
+        return false;
+    }
+    _mode = mode;
+    return true;
+}
+
 // set angle target in degrees
 // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
 void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)

libraries/AP_Mount/AP_Mount_Backend.h

@@ -69,12 +69,15 @@ public:
 
     // get angular velocity of mount. Only available on some backends
     virtual bool get_angular_velocity(Vector3f& rates) { return false; }
-    
+
+    // returns true if mode is a valid mode, false otherwise:
+    bool valid_mode(MAV_MOUNT_MODE mode) const;
+
     // get mount's mode
     enum MAV_MOUNT_MODE get_mode() const { return _mode; }
 
     // set mount's mode
-    void set_mode(enum MAV_MOUNT_MODE mode) { _mode = mode; }
+    bool set_mode(enum MAV_MOUNT_MODE mode);
 
     // set yaw_lock.  If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
     // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle