aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use new functions

Browse Source
This commit is contained in:
Andrew Tridgell 2012-12-19 13:36:00 +11:00
parent 06357c40f2
commit cf18534163
6 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.pde
View File

@ -1023,7 +1023,7 @@ static void update_current_flight_mode(void)
nav_pitch_cd = takeoff_pitch_cd;
} else {
nav_pitch_cd = (g_gps->ground_speed / (float)g.airspeed_cruise_cm) * takeoff_pitch_cd;
nav_pitch_cd = constrain(nav_pitch_cd, 500, takeoff_pitch_cd);
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
}
#if APM_CONTROL == DISABLED
@ -1058,7 +1058,7 @@ static void update_current_flight_mode(void)
if (!alt_control_airspeed()) {
// when not under airspeed control, don't allow
// down pitch in landing
nav_pitch_cd = constrain(nav_pitch_cd, 0, nav_pitch_cd);
nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd);
}
}
calc_throttle();
@ -1103,7 +1103,7 @@ static void update_current_flight_mode(void)
} else {
nav_pitch_cd = -(pitch_input * g.pitch_limit_min_cd);
}
nav_pitch_cd = constrain(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
nav_pitch_cd = constrain_int32(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd;
}

18
ArduPlane/Attitude.pde
View File

@ -172,7 +172,7 @@ static void calc_throttle()
g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch_cd / g.pitch_limit_min_cd;
}
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
} else {
// throttle control with airspeed compensation
// -------------------------------------------
@ -182,8 +182,8 @@ static void calc_throttle()
g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
g.throttle_min.get(), g.throttle_max.get());
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
g.throttle_min.get(), g.throttle_max.get());
}
}
@ -228,7 +228,7 @@ static void calc_nav_pitch()
} else {
nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
}
nav_pitch_cd = constrain(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
nav_pitch_cd = constrain_int32(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
}
@ -263,7 +263,7 @@ static void calc_nav_roll()
nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
#endif
nav_roll_cd = constrain(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
}
@ -292,7 +292,7 @@ static void throttle_slew_limit(int16_t last_throttle)
if (temp < 1) {
temp = 1;
}
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp);
g.channel_throttle.radio_out = constrain_int16(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp);
}
}
@ -448,9 +448,9 @@ static void set_servos(void)
g.channel_throttle.servo_out = 0;
#else
// convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
g.throttle_min.get(),
g.throttle_max.get());
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
g.throttle_min.get(),
g.throttle_max.get());
if (suppress_throttle()) {
// throttle is suppressed in auto mode

6
ArduPlane/GCS_Mavlink.pde
View File

@ -369,7 +369,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
aspeed = 0;
}
float throttle_norm = g.channel_throttle.norm_output() * 100;
throttle_norm = constrain(throttle_norm, -100, 100);
throttle_norm = constrain_int16(throttle_norm, -100, 100);
uint16_t throttle = ((uint16_t)(throttle_norm + 100)) / 2;
mavlink_msg_vfr_hud_send(
chan,
@ -469,7 +469,7 @@ static void NOINLINE send_wind(mavlink_channel_t chan)
chan,
degrees(atan2(-wind.y, -wind.x)), // use negative, to give
// direction wind is coming from
sqrt(sq(wind.x)+sq(wind.y)),
wind.length(),
wind.z);
}
@ -1783,7 +1783,7 @@ mission_failed:
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy));
float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360_cd(ToDeg(atan2(packet.vy, packet.vx)) * 100);
// set gps hil sensor

2
ArduPlane/commands.pde
View File

@ -99,7 +99,7 @@ static struct Location get_cmd_with_index(int16_t i)
// -------
static void set_cmd_with_index(struct Location temp, int16_t i)
{
i = constrain(i, 0, g.command_total.get());
i = constrain_int16(i, 0, g.command_total.get());
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask

6
ArduPlane/navigation.pde
View File

@ -119,9 +119,9 @@ static void calc_altitude_error()
// stay within a certain range
if(prev_WP.alt > next_WP.alt) {
target_altitude_cm = constrain(target_altitude_cm, next_WP.alt, prev_WP.alt);
target_altitude_cm = constrain_int32(target_altitude_cm, next_WP.alt, prev_WP.alt);
}else{
target_altitude_cm = constrain(target_altitude_cm, prev_WP.alt, next_WP.alt);
target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP.alt, next_WP.alt);
}
} else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
target_altitude_cm = next_WP.alt;
@ -195,7 +195,7 @@ static void update_crosstrack(void)
abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
// Meters we are off track line
crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;
nav_bearing_cd += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
}

2
ArduPlane/sensors.pde
View File

@ -69,7 +69,7 @@ void read_receiver_rssi(void)
{
rssi_analog_source->set_pin(g.rssi_pin);
float ret = rssi_analog_source->read_average();
receiver_rssi = constrain(ret, 0, 255);
receiver_rssi = constrain_int16(ret, 0, 255);
}