From 23a4dbd5aa24444afedb08fba992fc36234d18c4 Mon Sep 17 00:00:00 2001 From: Garrett Warren Date: Sun, 12 Jul 2020 21:07:57 -0400 Subject: [PATCH] tested fault protector --- params/shutdown_strings.yaml | 4 +- scripts/fault_protector.py | 14 ++-- test/test_fault_protector.txt | 149 ++++++++++++++++++++++++++++++++++ 3 files changed, 158 insertions(+), 9 deletions(-) create mode 100644 test/test_fault_protector.txt diff --git a/params/shutdown_strings.yaml b/params/shutdown_strings.yaml index 84f297b..469b1fb 100644 --- a/params/shutdown_strings.yaml +++ b/params/shutdown_strings.yaml @@ -1,5 +1,5 @@ # general info strings -prev_val: +prev_value: "\nINFO: value before cutoff: " change_cutoff: "\nINFO: if you would like to change the threshold, then edit @@ -41,6 +41,6 @@ acc: gyro: x: "\nSafety Failure: Crash predicted based on roll rate" y: "\nSafety Failure: Crash predicted based on pitch rate" - z: "Safety Failure: Crash predicted based on yaw rate" + z: "\nSafety Failure: Crash predicted based on yaw rate" altitude: "\nSafety Failure: Drone is flying too high" \ No newline at end of file diff --git a/scripts/fault_protector.py b/scripts/fault_protector.py index 2564bb0..4848b13 100644 --- a/scripts/fault_protector.py +++ b/scripts/fault_protector.py @@ -103,13 +103,13 @@ def check_accelerations(self, imu_msg): az = imu_msg.linear_acceleration.z if abs(ax) > self.thresholds["acceleration"]["x"]: self.shutdown = True - self.shutdown_cause += self.strs["acc"]["x"] + self.strs["change_cutoff"] + self.strs["prev_val"] + str(ax) + self.shutdown_cause += self.strs["acc"]["x"] + self.strs["change_cutoff"] + self.strs["prev_value"] + str(ax) if abs(ay) > self.thresholds["acceleration"]["y"]: self.shutdown = True - self.shutdown_cause += self.strs["acc"]["y"] + self.strs["change_cutoff"] + self.strs["prev_val"] + str(ay) + self.shutdown_cause += self.strs["acc"]["y"] + self.strs["change_cutoff"] + self.strs["prev_value"] + str(ay) if abs(az) > self.thresholds["acceleration"]["z"]: self.shutdown = True - self.shutdown_cause += self.strs["acc"]["z"] + self.strs["change_cutoff"] + self.strs["prev_val"] + str(az) + self.shutdown_cause += self.strs["acc"]["z"] + self.strs["change_cutoff"] + self.strs["prev_value"] + str(az) def check_angular_rates(self, imu_msg): roll_rate = imu_msg.angular_velocity.x @@ -117,13 +117,13 @@ def check_angular_rates(self, imu_msg): yaw_rate = imu_msg.angular_velocity.z if abs(roll_rate) > self.thresholds["angular_velocity"]["x"]: self.shutdown = True - self.shutdown_cause += self.strs["gyro"]["x"] + self.strs["change_cutoff"] + self.strs["prev_val"] + str(roll_rate) + self.shutdown_cause += self.strs["gyro"]["x"] + self.strs["change_cutoff"] + self.strs["prev_value"] + str(roll_rate) if abs(pitch_rate) > self.thresholds["angular_velocity"]["y"]: self.shutdown = True - self.shutdown_cause += self.strs["gyro"]["y"] + self.strs["change_cutoff"] + self.strs["prev_val"] + str(pitch_rate) + self.shutdown_cause += self.strs["gyro"]["y"] + self.strs["change_cutoff"] + self.strs["prev_value"] + str(pitch_rate) if abs(yaw_rate) > self.thresholds["angular_velocity"]["z"]: self.shutdown = True - self.shutdown_cause += self.strs["gyro"]["z"] + self.strs["change_cutoff"] + self.strs["prev_val"] + str(yaw_rate) + self.shutdown_cause += self.strs["gyro"]["z"] + self.strs["change_cutoff"] + self.strs["prev_value"] + str(yaw_rate) def check_altitude(self): if self.altitude > self.thresholds["altitude"]: @@ -132,7 +132,7 @@ def check_altitude(self): def check_battery(self, battery_voltage): if battery_voltage is not None and battery_voltage < self.thresholds["battery_voltage"]: - self.shutdown_cause += self.strs["battery"] + self.strs["prev_val"] + str(battery_voltage) + self.shutdown_cause += self.strs["battery"] + self.strs["prev_value"] + str(battery_voltage) self.shutdown = True def should_i_shutdown(self, mode, prev_mode, battery_voltage, imu_msg): diff --git a/test/test_fault_protector.txt b/test/test_fault_protector.txt new file mode 100644 index 0000000..aeb19c5 --- /dev/null +++ b/test/test_fault_protector.txt @@ -0,0 +1,149 @@ + +# test flight normal +rosbag: normal_liftoff +input: fly the drone with the pid tuned so that it quickly lifts to setpoint. fly around the map +expected: no safety failures +output: no safety failures + +# test flight slow +rosbag: slow_liftoff +input: fly the drone with the pid tuned so that it slowly lifts to setpoint. fly around the map +expected: no safety failures +output: no safety failures + +# test flight too fast +rosbag: n/a +input: fly the drone with the pid tuned so that it takes off too fast +expected: safety failure from z-axis acceleration +output: + Safety Failure: Crash predicted based on pitch rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -3.05892055539 + INFO: Sending DISARM command + +# test check_alititude +rosbag: check_alititude +input: arm drone close to ground. lift with hand above height threshold. +output: + Safety Failure: Drone is flying too high + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 0.507127568493 + INFO: Sending DISARM command + +# test check_accelerations + + ## test postive x-axis + rosbag: check_accel_x_pos + input: arm drone. use hand to thrust drone in positive x-axis + output: + Safety Failure: Crash predicted based on X-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 11.2605106868 + INFO: Sending DISARM command + + ## test negative x-axis + rosbag: check_accel_x_neg + input: arm drone. use hand to thrust drone in negative x-axis + output: + Safety Failure: Crash predicted based on X-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -23.6029466744 + INFO: Sending DISARM command + + ## test postive y-axis + rosbag: check_accel_y_pos + input: arm drone. use hand to thrust drone in positive y-axis + output: + Safety Failure: Crash predicted based on Y-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 21.0482279669 + INFO: Sending DISARM command + + ## test negative y-axis + rosbag: check_accel_y_neg + input: arm drone. use hand to thrust drone in negative y-axis + output: + Safety Failure: Crash predicted based on Y-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -16.128889755 + INFO: Sending DISARM command + + ## test postive z-axis + rosbag: check_altitude_z_pos + input: arm drone. use hand to thrust drone in positive z-axis + output: + Safety Failure: Crash predicted based on Z-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 18.283687766 + INFO: Sending DISARM command + + ## test negative z-axis + rosbag: check_accel_z_neg + input: arm drone. use hand to thrust drone in negative z-axis + output: + Safety Failure: Crash predicted based on Z-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -15.4114765005 + INFO: Sending DISARM command + + ## test pos x,y direction + rosbag: check_accel_x_y_pos + input: arm drone. use hand to thrust drone in positive x and y direction + output: + Safety Failure: Crash predicted based on X-axis acceleration + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 10.3531743647 + INFO: Sending DISARM command + +# test check_angular_velocities + + ## test positive roll + input: arm drone and roll hard right by hand + output: + Safety Failure: Crash predicted based on roll rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 6.18942309104 + INFO: Sending DISARM command + + ## test negative roll + input: arm drone and roll hard left by hand + output: + Safety Failure: Crash predicted based on roll rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -2.91523945217 + INFO: Sending DISARM command + + ## test positive pitch + input: arm drone and pitch nose up hard by hand + output: + Safety Failure: Crash predicted based on pitch rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 4.86890767357 + INFO: Sending DISARM command + + ## test negative pitch + input: arm drone and pitch nose down hard by hand + output: + Safety Failure: Crash predicted based on pitch rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -3.13265799573 + INFO: Sending DISARM command + + ## test clockwise yaw + input: arm drone and yaw hard clockwise by hand + expected: safety failure due to negative yaw rate (since math convention is ccw=positive yaw) + output: + Safety Failure: Crash predicted based on yaw rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: -3.03257311458 + INFO: Sending DISARM command + + ## test counter-clockwise yaw + input: arm drone and yaw hard counter-clockwise by hand + expected: safety failure due to postive yaw rate (since math convention is ccw=positive yaw) + output: + Safety Failure: Crash predicted based on yaw rate + INFO: if you would like to change the threshold, then edit the value in thresholds.yaml + INFO: value before cutoff: 4.81545352283 + INFO: Sending DISARM command +