From ef719b145bf9ef8bd370b521efe6c2b14145ead4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 12 Jan 2015 20:41:12 +0900 Subject: [PATCH] Mount: rename MODE to DFLT_MODE, move set_mode to backend --- libraries/AP_Mount/AP_Mount.cpp | 27 ++++++++++++--------------- libraries/AP_Mount/AP_Mount.h | 8 +++++--- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 68af160987..d36e154dc4 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -9,12 +9,12 @@ #include const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { - // @Param: MODE - // @DisplayName: Mount operation mode - // @Description: Camera or antenna mount operation mode - // @Values: 0:retract,1:neutral,2:MavLink_targeting,3:RC_targeting,4:GPS_point + // @Param: DEFLT_MODE + // @DisplayName: Mount default operating mode + // @Description: Mount default operating mode on startup and after control is returned from autopilot + // @Values: 0:Retracted,1:Neutral,2:MavLink Targeting,3:RC Targeting,4:GPS Point // @User: Standard - AP_GROUPINFO("MODE", 0, AP_Mount, state[0]._mode, MAV_MOUNT_MODE_RETRACT), // see MAV_MOUNT_MODE at ardupilotmega.h + AP_GROUPINFO("DEFLT_MODE", 0, AP_Mount, state[0]._default_mode, MAV_MOUNT_MODE_RC_TARGETING), // @Param: RETRACT_X // @DisplayName: Mount roll angle when in retracted position @@ -284,29 +284,26 @@ MAV_MOUNT_MODE AP_Mount::get_mode(uint8_t instance) const return MAV_MOUNT_MODE_RETRACT; } - return (enum MAV_MOUNT_MODE)state[instance]._mode.get(); + return state[instance]._mode; } // set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter // this operation requires 230us on an APM2, 60us on a Pixhawk/PX4 void AP_Mount::set_mode_to_default(uint8_t instance) { - // sanity check instance - if (instance >= AP_MOUNT_MAX_INSTANCES) { - return; - } - - // load instance's state from eeprom - state[instance]._mode.load(); + set_mode(instance, (enum MAV_MOUNT_MODE)state[instance]._default_mode.get()); } // set_mode - sets mount's mode void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) { // sanity check instance - if (instance < AP_MOUNT_MAX_INSTANCES) { - state[instance]._mode = (int8_t)mode; + if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) { + return; } + + // call backend's set_mode + _backends[instance]->set_mode(mode); } /// Change the configuration of the mount diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 3c8e7bf8b0..e6bbd93acf 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -78,10 +78,11 @@ public: enum MAV_MOUNT_MODE get_mode(uint8_t instance) const; // set_mode - sets mount's mode - void set_mode(enum MAV_MOUNT_MODE mode) { set_mode(_primary, mode); } + // returns true if mode is successfully set + void set_mode(enum MAV_MOUNT_MODE mode) { return set_mode(_primary, mode); } void set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode); - // set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter + // set_mode_to_default - restores the mode to it's default mode held in the MNT_DEFLT_MODE parameter // this operation requires 230us on an APM2, 60us on a Pixhawk/PX4 void set_mode_to_default() { set_mode_to_default(_primary); } void set_mode_to_default(uint8_t instance); @@ -122,7 +123,7 @@ private: struct mount_state { // Parameters AP_Int8 _type; // mount type (None, Servo or MAVLink, see MountType enum) - AP_Int8 _mode; // Retracted, Neutral, RC_Targeting, GPS Point + AP_Int8 _default_mode; // default mode on startup and when control is returned from autopilot AP_Int8 _stab_roll; // 1 = mount should stabilize earth-frame roll axis, 0 = no stabilization AP_Int8 _stab_tilt; // 1 = mount should stabilize earth-frame pitch axis AP_Int8 _stab_pan; // 1 = mount should stabilize earth-frame yaw axis @@ -146,6 +147,7 @@ private: AP_Float _roll_stb_lead; // roll lead control gain AP_Float _pitch_stb_lead; // pitch lead control gain + MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) struct Location _roi_target; // roi target location } state[AP_MOUNT_MAX_INSTANCES]; };