Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: factor out update_mnt_target_from_rc_target from servo, use it elsewhere

Browse Source 
this gives all backends the neutral-on-RC-failsafe behaviour
This commit is contained in:
Peter Barker 2024-11-09 14:47:30 +11:00 committed by Peter Barker
parent 500ec85e52
commit dcc04d685f
13 changed files with 48 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -100,20 +100,9 @@ void AP_Mount_Alexmos::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

23
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -66,6 +66,29 @@ bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
return true; return true;
} }
// called when mount mode is RC-targetting, updates the mnt_target object from RC inputs:
void AP_Mount_Backend::update_mnt_target_from_rc_target()
{
if (rc().in_rc_failsafe()) {
if (option_set(Options::NEUTRAL_ON_RC_FS)) {
mnt_target.angle_rad.set(_params.neutral_angles.get() * DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
return;
}
}
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
}
// set angle target in degrees // set angle target in degrees
// roll and pitch are in earth-frame // roll and pitch are in earth-frame
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame

3
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -253,6 +253,9 @@ protected:
}; };
bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; }
// called when mount mode is RC-targetting, updates the mnt_target object from RC inputs:
void update_mnt_target_from_rc_target();
// returns true if user has configured a valid roll angle range // returns true if user has configured a valid roll angle range
// allows user to disable roll even on 3-axis gimbal // allows user to disable roll even on 3-axis gimbal
bool roll_range_valid() const { return (_params.roll_angle_min < _params.roll_angle_max); } bool roll_range_valid() const { return (_params.roll_angle_min < _params.roll_angle_max); }

15
libraries/AP_Mount/AP_Mount_Gremsy.cpp
View File

@ -48,20 +48,9 @@ void AP_Mount_Gremsy::update()
} }
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -52,21 +52,10 @@ void AP_Mount_SToRM32::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
resend_now = true; resend_now = true;
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -49,21 +49,10 @@ void AP_Mount_SToRM32_serial::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
resend_now = true; resend_now = true;
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_Scripting.cpp
View File

@ -45,21 +45,10 @@ void AP_Mount_Scripting::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
target_loc_valid = false; target_loc_valid = false;
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

23
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -55,28 +55,9 @@ void AP_Mount_Servo::update()
} }
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's rc inputs or go to neutral or retracted targets if no rc update_mnt_target_from_rc_target();
if (rc().in_rc_failsafe()) {
if (option_set(Options::NEUTRAL_ON_RC_FS)) {
mnt_target.angle_rad.set(_angle_bf_output_rad, false);
mnt_target.target_type = MountTargetType::ANGLE;
}
} else {
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -121,20 +121,9 @@ void AP_Mount_Siyi::update()
break; break;
} }
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

14
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -65,20 +65,10 @@ void AP_Mount_SoloGimbal::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
_gimbal.set_lockedToBody(false); _gimbal.set_lockedToBody(false);
MountTarget rc_target; update_mnt_target_from_rc_target();
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_Topotek.cpp
View File

@ -156,20 +156,9 @@ void AP_Mount_Topotek::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_Viewpro.cpp
View File

@ -94,20 +94,9 @@ void AP_Mount_Viewpro::update()
break; break;
// RC radio manual angle control, but with stabilization from the AHRS // RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT:

15
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -135,20 +135,9 @@ void AP_Mount_Xacti::update()
break; break;
} }
case MAV_MOUNT_MODE_RC_TARGETING: { case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's RC inputs update_mnt_target_from_rc_target();
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break; break;
}
// point mount to a GPS point given by the mission planner // point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: case MAV_MOUNT_MODE_GPS_POINT: