Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: backend record RC rate targets

Browse Source
This commit is contained in:
Randy Mackay 2022-06-02 16:03:41 +09:00
parent f157e5a073
commit 8092697c1a
2 changed files with 22 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -4,6 +4,8 @@
extern const AP_HAL::HAL& hal;
#define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate
// set_angle_targets - sets angle targets in degrees
void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan)
{
@ -114,13 +116,17 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli
return true;
}
void AP_Mount_Backend::rate_input_rad(float &out, const RC_Channel *chan, float min, float max) const
// update rate and angle targets from RC input
// current angle target (in radians) should be provided in angle_rad target
// rate and angle targets are returned in rate_rads and angle_rad arguments
void AP_Mount_Backend::update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_rad, float angle_max_rad) const
{
if ((chan == nullptr) || (chan->get_radio_in() == 0)) {
rate_rads = 0;
return;
}
out += chan->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
out = constrain_float(out, radians(min*0.01f), radians(max*0.01f));
rate_rads = chan->norm_input_dz() * 0.005f * _frontend._joystick_speed;
angle_rad = constrain_float(angle_rad + (rate_rads * AP_MOUNT_UPDATE_DT), radians(angle_min_rad*0.01f), radians(angle_max_rad*0.01f));
}
// update_targets_from_rc - updates angle targets using input from receiver
@ -131,20 +137,12 @@ void AP_Mount_Backend::update_targets_from_rc()
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) {
if (_frontend._joystick_speed > 0) {
// 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);
update_rate_and_angle_from_rc(roll_ch, _rate_target_rads.x, _angle_ef_target_rad.x, _state._roll_angle_min, _state._roll_angle_max);
update_rate_and_angle_from_rc(tilt_ch, _rate_target_rads.y, _angle_ef_target_rad.y, _state._tilt_angle_min, _state._tilt_angle_max);
update_rate_and_angle_from_rc(pan_ch, _rate_target_rads.z, _angle_ef_target_rad.z, _state._pan_angle_min, _state._pan_angle_max);
_rate_target_rads_valid = true;
} else {
// allow pilot rate input to come directly from an RC_Channel
if ((roll_ch != nullptr) && (roll_ch->get_radio_in() != 0)) {
@ -156,6 +154,8 @@ void AP_Mount_Backend::update_targets_from_rc()
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);
}
// not using rate input
_rate_target_rads_valid = false;
}
}

7
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -118,10 +118,15 @@ protected:
AP_Mount::mount_state &_state; // references to the parameters and state for this backend
uint8_t _instance; // this instance's number
Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and vehicle-relative pan angles in radians
Vector3f _rate_target_rads; // desired roll, pitch, yaw rate in radians/sec
bool _rate_target_rads_valid;// true if _rate_target_rads should can be used (e.g. RC input is using rate control)
private:
void rate_input_rad(float &out, const RC_Channel *ch, float min, float max) const;
// update rate and angle targets from RC input
// current angle target (in radians) should be provided in angle_rad target
// rate and angle targets are returned in rate_rads and angle_rad arguments
void update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_rad, float angle_max_rad) const;
};
#endif // HAL_MOUNT_ENABLED