inline void BP_Mount_Component::get_valid_pwm_from_channel(uint8_t rc_in, uint16_t* pwm)
{
#define rc_ch(i) RC_Channel::rc_channel(i-1)
if (rc_in && (rc_ch(rc_in))) {
*pwm = rc_ch(rc_in)->get_radio_in();
} else
*pwm = 1500;
}
// 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:
{
#define rc_ch(i) RC_Channel::rc_channel(i-1)
#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)) {
_roll_control_angle += rc_ch(_roll_rc_in)->norm_input() * 0.0001f * _joystick_speed;
if (_roll_control_angle < radians(_roll_angle_min*0.01f)) _roll_control_angle = radians(_roll_angle_min*0.01f);
if (_roll_control_angle > radians(_roll_angle_max*0.01f)) _roll_control_angle = radians(_roll_angle_max*0.01f);
}
if (_tilt_rc_in && (rc_ch(_tilt_rc_in))) {
_tilt_control_angle += rc_ch(_tilt_rc_in)->norm_input() * 0.0001f * _joystick_speed;
if (_tilt_control_angle < radians(_tilt_angle_min*0.01f)) _tilt_control_angle = radians(_tilt_angle_min*0.01f);
if (_tilt_control_angle > radians(_tilt_angle_max*0.01f)) _tilt_control_angle = radians(_tilt_angle_max*0.01f);
}
if (_pan_rc_in && (rc_ch(_pan_rc_in))) {
_pan_control_angle += rc_ch(_pan_rc_in)->norm_input() * 0.0001f * _joystick_speed;
if (_pan_control_angle < radians(_pan_angle_min*0.01f)) _pan_control_angle = radians(_pan_angle_min*0.01f);
if (_pan_control_angle > radians(_pan_angle_max*0.01f)) _pan_control_angle = radians(_pan_angle_max*0.01f);
}
} else {
#endif
// allow pilot position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch(_roll_rc_in))) {
_roll_control_angle = angle_input_rad(rc_ch(_roll_rc_in), _roll_angle_min, _roll_angle_max);
}
if (_tilt_rc_in && (rc_ch(_tilt_rc_in))) {
_tilt_control_angle = angle_input_rad(rc_ch(_tilt_rc_in), _tilt_angle_min, _tilt_angle_max);
}
if (_pan_rc_in && (rc_ch(_pan_rc_in))) {
_pan_control_angle = angle_input_rad(rc_ch(_pan_rc_in), _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(_ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
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.1f, _roll_angle_max*0.1f);
move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1f, _tilt_angle_max*0.1f);
move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1f, _pan_angle_max*0.1f);
}
