From 79cad28a25c44d531b256d883afcddbdcce23865 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Jan 2015 17:23:16 +1100 Subject: [PATCH] AP_Mount: simplify some uses of frontend --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 2 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 26 ++++++++++++------------- libraries/AP_Mount/AP_Mount_Backend.h | 3 +++ libraries/AP_Mount/AP_Mount_MAVLink.cpp | 2 +- libraries/AP_Mount/AP_Mount_Servo.cpp | 4 ++-- 5 files changed, 20 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 6c9d58d3d3..56ca31b8b6 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -23,7 +23,7 @@ void AP_Mount_Alexmos::update() read_incoming(); // read the incoming messages from the gimbal // update based on mount mode - switch(_frontend.get_mode(_instance)) { + switch(get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism case MAV_MOUNT_MODE_RETRACT: control_axis(_state._retract_angles.get(), true); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index b374773096..9e1d415ab8 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -8,7 +8,7 @@ extern const AP_HAL::HAL& hal; void AP_Mount_Backend::set_roi_target(const struct Location &target_loc) { // set the target gps location - _frontend.state[_instance]._roi_target = target_loc; + _state._roi_target = target_loc; // set the mode to GPS tracking mode _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT); @@ -24,9 +24,9 @@ void AP_Mount_Backend::configure_msg(mavlink_message_t* msg) _frontend.set_mode(_instance,(enum MAV_MOUNT_MODE)packet.mount_mode); // set which axis are stabilized - _frontend.state[_instance]._stab_roll = packet.stab_roll; - _frontend.state[_instance]._stab_tilt = packet.stab_pitch; - _frontend.state[_instance]._stab_pan = packet.stab_yaw; + _state._stab_roll = packet.stab_roll; + _state._stab_tilt = packet.stab_pitch; + _state._stab_pan = packet.stab_yaw; } // control_msg - process MOUNT_CONTROL messages received from GCS @@ -74,35 +74,35 @@ void AP_Mount_Backend::update_targets_from_rc() { #define rc_ch(i) RC_Channel::rc_channel(i-1) - uint8_t roll_rc_in = _frontend.state[_instance]._roll_rc_in; - uint8_t tilt_rc_in = _frontend.state[_instance]._tilt_rc_in; - uint8_t pan_rc_in = _frontend.state[_instance]._pan_rc_in; + uint8_t roll_rc_in = _state._roll_rc_in; + uint8_t tilt_rc_in = _state._tilt_rc_in; + uint8_t pan_rc_in = _state._pan_rc_in; // if joystick_speed is defined then pilot input defines a rate of change of the angle if (_frontend._joystick_speed) { // allow pilot speed position input to come directly from an RC_Channel if (roll_rc_in && rc_ch(roll_rc_in)) { _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.x, radians(_frontend.state[_instance]._roll_angle_min*0.01f), radians(_frontend.state[_instance]._roll_angle_max*0.01f)); + constrain_float(_angle_ef_target_rad.x, radians(_state._roll_angle_min*0.01f), radians(_state._roll_angle_max*0.01f)); } if (tilt_rc_in && (rc_ch(tilt_rc_in))) { _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.y, radians(_frontend.state[_instance]._tilt_angle_min*0.01f), radians(_frontend.state[_instance]._tilt_angle_max*0.01f)); + constrain_float(_angle_ef_target_rad.y, radians(_state._tilt_angle_min*0.01f), radians(_state._tilt_angle_max*0.01f)); } if (pan_rc_in && (rc_ch(pan_rc_in))) { _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.z, radians(_frontend.state[_instance]._pan_angle_min*0.01f), radians(_frontend.state[_instance]._pan_angle_max*0.01f)); + constrain_float(_angle_ef_target_rad.z, radians(_state._pan_angle_min*0.01f), radians(_state._pan_angle_max*0.01f)); } } else { // allow pilot position input to come directly from an RC_Channel if (roll_rc_in && (rc_ch(roll_rc_in))) { - _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _frontend.state[_instance]._roll_angle_min, _frontend.state[_instance]._roll_angle_max); + _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _state._roll_angle_min, _state._roll_angle_max); } if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _frontend.state[_instance]._tilt_angle_min, _frontend.state[_instance]._tilt_angle_max); + _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _state._tilt_angle_min, _state._tilt_angle_max); } if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _frontend.state[_instance]._pan_angle_min, _frontend.state[_instance]._pan_angle_max); + _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _state._pan_angle_min, _state._pan_angle_max); } } } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 8c587b52e8..df279a409a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -74,6 +74,9 @@ protected: // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan); + // get the mount mode from frontend + MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); } + AP_Mount &_frontend; // reference to the front end which holds parameters AP_Mount::mount_state &_state; // refernce to the parameters and state for this backend uint8_t _instance; // this instance's number diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 89be19ad36..391c706f40 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -21,7 +21,7 @@ void AP_Mount_MAVLink::update() } // update based on mount mode - switch(_frontend.get_mode(_instance)) { + switch(get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism case MAV_MOUNT_MODE_RETRACT: send_angle_target(_state._retract_angles.get(), true); diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index f775ae63bf..d827f9aaa7 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -36,7 +36,7 @@ void AP_Mount_Servo::update() _last_check_servo_map_ms = now; } - switch(_frontend.get_mode(_instance)) { + switch(get_mode()) { // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage case MAV_MOUNT_MODE_RETRACT: { @@ -84,7 +84,7 @@ void AP_Mount_Servo::update() } // move mount to a "retracted position" into the fuselage with a fourth servo - bool mount_open_new = (_frontend.get_mode(_instance) == MAV_MOUNT_MODE_RETRACT) ? 0 : 1; + bool mount_open_new = (get_mode() == MAV_MOUNT_MODE_RETRACT) ? 0 : 1; if (mount_open != mount_open_new) { mount_open = mount_open_new; move_servo(_open_idx, mount_open_new, 0, 1);