// update_targets_from_rc - updates angle targets using input from receiver void AP_Mount_Backend::update_targets_from_rc() { const RC_Channel *roll_ch = rc().channel(_state._roll_rc_in - 1); const RC_Channel *tilt_ch = rc().channel(_state._tilt_rc_in - 1); const RC_Channel *pan_ch = rc().channel(_state._pan_rc_in - 1); // if joystick_speed is defined then pilot input defines a rate of change of the angle if (_frontend._joystick_speed) { // allow pilot position input to come directly from an RC_Channel rate_input_rad(_angle_ef_target_rad.x, roll_ch, _state._roll_angle_min, _state._roll_angle_max); rate_input_rad(_angle_ef_target_rad.y, tilt_ch, _state._tilt_angle_min, _state._tilt_angle_max); rate_input_rad(_angle_ef_target_rad.z, pan_ch, _state._pan_angle_min, _state._pan_angle_max); } else { // allow pilot rate input to come directly from an RC_Channel if ((roll_ch != nullptr) && (roll_ch->get_radio_in() != 0)) { _angle_ef_target_rad.x = angle_input_rad(roll_ch, _state._roll_angle_min, _state._roll_angle_max); } if ((tilt_ch != nullptr) && (tilt_ch->get_radio_in() != 0)) { _angle_ef_target_rad.y = angle_input_rad(tilt_ch, _state._tilt_angle_min, _state._tilt_angle_max); } if ((pan_ch != nullptr) && (pan_ch->get_radio_in() != 0)) { _angle_ef_target_rad.z = angle_input_rad(pan_ch, _state._pan_angle_min, _state._pan_angle_max); } } }
// update_targets_from_rc - updates angle targets using input from receiver void AP_Mount_Backend::update_targets_from_rc() { #define rc_ch(i) RC_Channels::rc_channel(i-1) 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; _angle_ef_target_rad.x = 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; _angle_ef_target_rad.y = 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; _angle_ef_target_rad.z = 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), _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), _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), _state._pan_angle_min, _state._pan_angle_max); } } }
/// This one should be called periodically void AP_Mount::update_mount_position() { #if MNT_RETRACT_OPTION == ENABLED static bool mount_open = 0; // 0 is closed #endif switch((enum MAV_MOUNT_MODE)_mount_mode.get()) { #if MNT_RETRACT_OPTION == ENABLED // 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: { Vector3f vec = _retract_angles.get(); _roll_angle = vec.x; _tilt_angle = vec.y; _pan_angle = vec.z; break; } #endif // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { Vector3f vec = _neutral_angles.get(); _roll_angle = vec.x; _tilt_angle = vec.y; _pan_angle = vec.z; break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: { Vector3f vec = _control_angles.get(); _roll_control_angle = radians(vec.x); _tilt_control_angle = radians(vec.y); _pan_control_angle = radians(vec.z); stabilize(); break; } // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { #if MNT_JSTICK_SPD_OPTION == ENABLED if (_joystick_speed) { // for spring loaded joysticks // allow pilot speed position input to come directly from an RC_Channel if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { //_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed; _roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01); if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01); } if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) { //_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed; _tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01); if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01); } if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { //_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed; _pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01); if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01); } } else { #endif // allow pilot position input to come directly from an RC_Channel if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { _roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max); } if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) { _tilt_control_angle = angle_input_rad(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max); } if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { _pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max); } #if MNT_JSTICK_SPD_OPTION == ENABLED } #endif stabilize(); break; } #if MNT_GPSPOINT_OPTION == ENABLED // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: { if(_gps->fix) { calc_GPS_target_angle(&_target_GPS_location); stabilize(); } break; } #endif default: //do nothing break; } #if MNT_RETRACT_OPTION == ENABLED // move mount to a "retracted position" into the fuselage with a fourth servo bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==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); } #endif // write the results to the servos move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1); move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1); }