CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Allow using nans in set_rates

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-08 08:51:58 +03:00
parent 7980773d95
commit 325d1c317b
2 changed files with 38 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
clover/src/simple_offboard.cpp
View File

@@ -685,10 +685,6 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
ENSURE_FINITE(pitch);
ENSURE_FINITE(roll);
ENSURE_FINITE(thrust);
} else if (sp_type == RATES) {
ENSURE_FINITE(pitch_rate);
ENSURE_FINITE(roll_rate);
ENSURE_FINITE(thrust);
}
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
@@ -811,14 +807,16 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
}
if (sp_type == ATTITUDE || sp_type == RATES) {
thrust_msg.thrust = thrust;
// keep current value if NaN
thrust_msg.thrust = isnan(thrust) ? thrust_msg.thrust : thrust;
}
if (sp_type == RATES) {
setpoint_yaw_type = YAW_RATE;
setpoint_rates.x = roll_rate;
setpoint_rates.y = pitch_rate;
setpoint_rates.z = yaw_rate;
// keep current values if NaN
setpoint_rates.x = isnan(roll_rate) ? setpoint_rates.x : roll_rate;
setpoint_rates.y = isnan(pitch_rate) ? setpoint_rates.y : pitch_rate;
setpoint_rates.z = isnan(yaw_rate) ? setpoint_rates.z : yaw_rate; // TODO: consider always
}
wait_armed = auto_arm;

32
clover/test/offboard.py
View File

@@ -18,6 +18,7 @@ def get_state():
def test_offboard(node):
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
res = navigate()
assert res.success == False
@@ -94,3 +95,34 @@ def test_offboard(node):
assert state.pitch == approx(0.2)
assert state.yaw == approx(0.3)
assert state.thrust == approx(0.5)
# test set_rates
res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.5)
assert res.success == True
state = get_state()
assert state.mode == State.MODE_RATES
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
assert state.roll_rate == approx(0)
assert state.pitch_rate == approx(0)
assert state.yaw_rate == approx(0.3)
assert state.thrust == approx(0.5)
res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4)
assert res.success == True
state = get_state()
assert state.mode == State.MODE_RATES
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
assert state.roll_rate == approx(0.3)
assert state.pitch_rate == approx(0.2)
assert state.yaw_rate == approx(0.1)
assert state.thrust == approx(0.4)
res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3)
assert res.success == True
state = get_state()
assert state.mode == State.MODE_RATES
assert state.yaw_mode == State.YAW_MODE_YAW_RATE
assert state.roll_rate == approx(0.3)
assert state.pitch_rate == approx(0.2)
assert state.yaw_rate == approx(0.1)
assert state.thrust == approx(0.3)