From 82c604dd67f3742ca9e567a51e17b8e6649ae8ae Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Apr 2013 23:44:57 +1000 Subject: [PATCH] Copter: more constrain fixes --- ArduCopter/control_modes.pde | 2 +- ArduCopter/navigation.pde | 2 +- ArduCopter/sensors.pde | 2 +- ArduCopter/setup.pde | 2 +- ArduCopter/system.pde | 2 +- ArduCopter/toy.pde | 8 ++++---- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index d053031982..22715340f9 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -139,7 +139,7 @@ static void read_trim_switch() // set the next_WP (home is stored at 0) // max out at 100 since I think we need to stay under the EEPROM limit - CH7_wp_index = constrain(CH7_wp_index, 1, 100); + CH7_wp_index = constrain_int16(CH7_wp_index, 1, 100); if(g.rc_3.control_in > 0) { // set our location ID to 16, MAV_CMD_NAV_WAYPOINT diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 5cdfc1243e..1c7d560aa5 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -200,7 +200,7 @@ static void reset_nav_params(void) // assumes it is called at 100hz so centi-degrees and update rate cancel each other out static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t deg_per_sec) { - return wrap_360_cd(current_yaw + constrain(wrap_180_cd(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec)); + return wrap_360_cd(current_yaw + constrain_int16(wrap_180_cd(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec)); } diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 55a300b65d..f38cbe2157 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -144,5 +144,5 @@ void read_receiver_rssi(void) { rssi_analog_source->set_pin(g.rssi_pin); float ret = rssi_analog_source->read_latest(); - receiver_rssi = constrain(ret, 0, 255); + receiver_rssi = constrain_int16(ret, 0, 255); } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index acbe92ec2b..e2030156ad 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -331,7 +331,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) if (_oldSwitchPosition != _switchPosition) { mode = flight_modes[_switchPosition]; - mode = constrain(mode, 0, NUM_MODES-1); + mode = constrain_int16(mode, 0, NUM_MODES-1); // update the user print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index aed769d8ca..738af97b01 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -364,7 +364,7 @@ static void set_mode(uint8_t mode) } control_mode = mode; - control_mode = constrain(control_mode, 0, NUM_MODES - 1); + control_mode = constrain_int16(control_mode, 0, NUM_MODES - 1); // if we change modes, we must clear landed flag set_land_complete(false); diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index 8b2d2b7646..8bd2b0a509 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -128,8 +128,8 @@ void roll_pitch_toy() yy = abs(yaw_rate / 500); // constrain to lookup Array range - xx = constrain(xx, 0, 3); - yy = constrain(yy, 0, 8); + xx = constrain_int16(xx, 0, 3); + yy = constrain_int16(yy, 0, 8); roll_rate = toy_lookup[yy * 4 + xx]; @@ -140,12 +140,12 @@ void roll_pitch_toy() } int16_t roll_limit = 4500 / g.toy_yaw_rate; - roll_rate = constrain(roll_rate, -roll_limit, roll_limit); + roll_rate = constrain_int16(roll_rate, -roll_limit, roll_limit); #elif TOY_MIXER == TOY_LINEAR_MIXER roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30; //cliSerial->printf("roll_rate: %d\n",roll_rate); - roll_rate = constrain(roll_rate, -2000, 2000); + roll_rate = constrain_int32(roll_rate, -2000, 2000); #elif TOY_MIXER == TOY_EXTERNAL_MIXER // JKR update to allow external roll/yaw mixing