L1_Control: constrain the roll to handle floating point errors

2013-04-15 14:29:39 +10:00 · 2013-04-15 14:29:39 +10:00 · 97bfd16bd1
commit 97bfd16bd1
parent 83dc7dbc92
1 changed files with 2 additions and 1 deletions
--- a/libraries/AP_L1_Control/AP_L1_Control.cpp
+++ b/libraries/AP_L1_Control/AP_L1_Control.cpp
@ -37,8 +37,9 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] PROGMEM = {
 */
 int32_t AP_L1_Control::nav_roll_cd(void)
 {
-	int32_t ret;	
+	float ret;	
 	ret = degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81
+	ret = constrain(ret, -9000, 9000);
 	return ret;
 }