Mount: rename MODE to DFLT_MODE, move set_mode to backend

This commit is contained in:
Randy Mackay 2015-01-12 20:41:12 +09:00 committed by Andrew Tridgell
parent 6c766051e8
commit ef719b145b
2 changed files with 17 additions and 18 deletions

View File

@ -9,12 +9,12 @@
#include <AP_Mount_MAVLink.h> #include <AP_Mount_MAVLink.h>
const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
// @Param: MODE // @Param: DEFLT_MODE
// @DisplayName: Mount operation mode // @DisplayName: Mount default operating mode
// @Description: Camera or antenna mount operation mode // @Description: Mount default operating mode on startup and after control is returned from autopilot
// @Values: 0:retract,1:neutral,2:MavLink_targeting,3:RC_targeting,4:GPS_point // @Values: 0:Retracted,1:Neutral,2:MavLink Targeting,3:RC Targeting,4:GPS Point
// @User: Standard // @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 // @Param: RETRACT_X
// @DisplayName: Mount roll angle when in retracted position // @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 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 // 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 // this operation requires 230us on an APM2, 60us on a Pixhawk/PX4
void AP_Mount::set_mode_to_default(uint8_t instance) void AP_Mount::set_mode_to_default(uint8_t instance)
{ {
// sanity check instance set_mode(instance, (enum MAV_MOUNT_MODE)state[instance]._default_mode.get());
if (instance >= AP_MOUNT_MAX_INSTANCES) {
return;
}
// load instance's state from eeprom
state[instance]._mode.load();
} }
// set_mode - sets mount's mode // set_mode - sets mount's mode
void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode)
{ {
// sanity check instance // sanity check instance
if (instance < AP_MOUNT_MAX_INSTANCES) { if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) {
state[instance]._mode = (int8_t)mode; return;
} }
// call backend's set_mode
_backends[instance]->set_mode(mode);
} }
/// Change the configuration of the mount /// Change the configuration of the mount

View File

@ -78,10 +78,11 @@ public:
enum MAV_MOUNT_MODE get_mode(uint8_t instance) const; enum MAV_MOUNT_MODE get_mode(uint8_t instance) const;
// set_mode - sets mount's mode // 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); 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 // 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() { set_mode_to_default(_primary); }
void set_mode_to_default(uint8_t instance); void set_mode_to_default(uint8_t instance);
@ -122,7 +123,7 @@ private:
struct mount_state { struct mount_state {
// Parameters // Parameters
AP_Int8 _type; // mount type (None, Servo or MAVLink, see MountType enum) 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_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_tilt; // 1 = mount should stabilize earth-frame pitch axis
AP_Int8 _stab_pan; // 1 = mount should stabilize earth-frame yaw 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 _roll_stb_lead; // roll lead control gain
AP_Float _pitch_stb_lead; // pitch 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 struct Location _roi_target; // roi target location
} state[AP_MOUNT_MAX_INSTANCES]; } state[AP_MOUNT_MAX_INSTANCES];
}; };