Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: Crash checks for Balance Bot

This commit is contained in:
Ebin 2018-06-21 18:20:28 +05:30 committed by Randy Mackay
parent 2780d1715c
commit 73e6ce18a0
3 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APMrover2/Parameters.cpp
View File

@ -556,6 +556,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5), AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5),
// @Param: BAL_PITCH_CRASH
// @DisplayName: BalanceBot Crash Pitch
// @Description: Pitch angle for crash check
// @Units: deg
// @Range: 0 60
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("BAL_PITCH_CRASH", 22, ParametersG2, bal_pitch_crash, 30),
AP_GROUPEND AP_GROUPEND
}; };

3
APMrover2/Parameters.h
View File

@ -344,6 +344,9 @@ public:
// pitch angle at 100% throttle // pitch angle at 100% throttle
AP_Float bal_pitch_max; AP_Float bal_pitch_max;
// balance bot pitch for crash check
AP_Float bal_pitch_crash;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

11
APMrover2/crash_check.cpp
View File

@ -12,6 +12,17 @@ void Rover::crash_check()
{ {
static uint16_t crash_counter; // number of iterations vehicle may have been crashed static uint16_t crash_counter; // number of iterations vehicle may have been crashed
if (is_balancebot() && arming.is_armed()) {
// Crashed if pitch > bal_pitch_crash
if (fabsf(ahrs.pitch) > radians(g2.bal_pitch_crash)) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Balance Bot crashed");
disarm_motors();
}
}
// return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode
if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) { if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) {
crash_counter = 0; crash_counter = 0;