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

Copter: replaced constrain() with constrain_float()

This commit is contained in:
Andrew Tridgell 2013-05-02 10:26:49 +10:00
parent b13406859f
commit 25c576cad7
6 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/ArduCopter.pde
View File

@ -1963,13 +1963,13 @@ static void update_trig(void){
cos_pitch_x = safe_sqrt(1 - (temp.c.x * temp.c.x)); // level = 1
cos_roll_x = temp.c.z / cos_pitch_x; // level = 1
cos_pitch_x = constrain(cos_pitch_x, 0, 1.0);
// this relies on constrain() of infinity doing the right thing,
cos_pitch_x = constrain_float(cos_pitch_x, 0, 1.0);
// this relies on constrain_float() of infinity doing the right thing,
// which it does do in avr-libc
cos_roll_x = constrain(cos_roll_x, -1.0, 1.0);
cos_roll_x = constrain_float(cos_roll_x, -1.0, 1.0);
sin_yaw = constrain(yawvector.y, -1.0, 1.0);
cos_yaw = constrain(yawvector.x, -1.0, 1.0);
sin_yaw = constrain_float(yawvector.y, -1.0, 1.0);
cos_yaw = constrain_float(yawvector.x, -1.0, 1.0);
// added to convert earth frame to body frame for rate controllers
sin_pitch = -temp.c.x;

28
ArduCopter/Attitude.pde
View File

@ -407,7 +407,7 @@ get_heli_rate_yaw(int32_t target_rate)
ff = g.heli_yaw_ff*target_rate;
output = p + i + d + ff;
output = constrain(output, -4500, 4500);
output = constrain_float(output, -4500, 4500);
#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
@ -745,7 +745,7 @@ static void update_throttle_cruise(int16_t throttle)
static int16_t get_angle_boost(int16_t throttle)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0f - constrain(angle_boost_factor, .5f, 1.0f);
angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f);
int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0);
// to allow logging of angle boost
@ -761,13 +761,13 @@ static int16_t get_angle_boost(int16_t throttle)
float temp = cos_pitch_x * cos_roll_x;
int16_t throttle_out;
temp = constrain(temp, 0.5f, 1.0f);
temp = constrain_float(temp, 0.5f, 1.0f);
// reduce throttle if we go inverted
temp = constrain(9000-max(labs(ahrs.roll_sensor),labs(ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
temp = constrain_float(9000-max(labs(ahrs.roll_sensor),labs(ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
// apply scale and constrain throttle
throttle_out = constrain((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000);
throttle_out = constrain_float((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000);
// to allow logging of angle boost
angle_boost = throttle_out - throttle;
@ -833,7 +833,7 @@ get_throttle_accel(int16_t z_target_accel)
z_accel_error = 0;
} else {
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164f * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
z_accel_error = z_accel_error + 0.11164f * (constrain_float(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
}
last_call_ms = now;
@ -849,7 +849,7 @@ get_throttle_accel(int16_t z_target_accel)
//
// limit the rate
output = constrain(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
@ -1020,7 +1020,7 @@ get_throttle_rate(float z_target_speed)
// limit loiter & waypoint navigation from causing too much lean
// To-Do: ensure that this limit is cleared when this throttle controller is not running so that loiter is not left constrained for Position mode
wp_nav.set_angle_limit(4500 - constrain((z_rate_error - 100) * 10, 0, 3500));
wp_nav.set_angle_limit(4500 - constrain_float((z_rate_error - 100) * 10, 0, 3500));
// update throttle cruise
// TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration
@ -1056,7 +1056,7 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
desired_rate = 0;
}
desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);
desired_rate = constrain_float(desired_rate, min_climb_rate, max_climb_rate);
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
@ -1073,10 +1073,10 @@ static void
get_throttle_althold_with_slew(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
// limit target altitude change
controller_desired_alt += constrain(target_alt-controller_desired_alt, min_climb_rate*0.02f, max_climb_rate*0.02f);
controller_desired_alt += constrain_float(target_alt-controller_desired_alt, min_climb_rate*0.02f, max_climb_rate*0.02f);
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
get_throttle_althold(controller_desired_alt, min_climb_rate-250, max_climb_rate+250); // 250 is added to give head room to alt hold controller
}
@ -1090,7 +1090,7 @@ get_throttle_rate_stabilized(int16_t target_rate)
controller_desired_alt += target_rate * 0.02f;
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
// update target altitude for reporting purposes
set_target_alt_for_reporting(controller_desired_alt);
@ -1150,11 +1150,11 @@ get_throttle_surface_tracking(int16_t target_rate)
target_sonar_alt += target_rate * 0.02f;
distance_error = (target_sonar_alt-sonar_alt);
sonar_induced_slew_rate = constrain(fabsf(g.sonar_gain * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);
sonar_induced_slew_rate = constrain_float(fabsf(g.sonar_gain * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);
// do not let target altitude get too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
target_sonar_alt = constrain(target_sonar_alt,sonar_alt-750,sonar_alt+750);
target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-750,sonar_alt+750);
controller_desired_alt = current_loc.alt+(target_sonar_alt-sonar_alt);
// update target altitude for reporting purposes

6
ArduCopter/GCS_Mavlink.pde
View File

@ -1801,7 +1801,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain(v, -2147483648.0, 2147483647.0);
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
#if LOGGING_ENABLED == ENABLED
@ -1811,7 +1811,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain(v, -32768, 32767);
v = constrain_float(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
#if LOGGING_ENABLED == ENABLED
@ -1821,7 +1821,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain(v, -128, 127);
v = constrain_float(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter

2
ArduCopter/commands.pde
View File

@ -68,7 +68,7 @@ static struct Location get_cmd_with_index(int i)
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.command_total.get());
i = constrain_float(i, 0, g.command_total.get());
//cliSerial->printf("set_command: %d with id: %d\n", i, temp.id);
// store home as 0 altitude!!!

2
ArduCopter/commands_logic.pde
View File

@ -658,7 +658,7 @@ static void do_yaw()
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - nav_yaw) / 100) / command_cond_queue.lat;
yaw_look_at_heading_slew = constrain(turn_rate, 1, 360); // deg / sec
yaw_look_at_heading_slew = constrain_float(turn_rate, 1, 360); // deg / sec
}
// set yaw mode

2
ArduCopter/setup.pde
View File

@ -576,7 +576,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
// calculate scaling for throttle
throttle_pct = (float)g.rc_3.control_in / 1000.0f;
throttle_pct = constrain(throttle_pct,0.0f,1.0f);
throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
// if throttle is zero, update base x,y,z values
if( throttle_pct == 0.0f ) {