5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00

AP_Mount: simplify some uses of frontend

This commit is contained in:
Andrew Tridgell 2015-01-29 17:23:16 +11:00
parent c05f36d29b
commit 79cad28a25
5 changed files with 20 additions and 17 deletions

View File

@ -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);

View File

@ -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);
}
}
}

View File

@ -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

View File

@ -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);

View File

@ -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);