remove unused climb_rate code

This commit is contained in:
Andrew Tridgell 2011-09-18 19:01:21 +10:00
parent c2126e127d
commit e7bc74a38e
3 changed files with 1 additions and 37 deletions

View File

@ -260,7 +260,6 @@ const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
static long hold_course = -1; // deg * 100 dir of plane

View File

@ -213,7 +213,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
(dcm.yaw_sensor / 100) % 360,
(int)g.channel_throttle.servo_out,
current_loc.alt / 100.0,
climb_rate);
0);
}
#if HIL_MODE != HIL_MODE_ATTITUDE

View File

@ -57,38 +57,3 @@ static void add_altitude_data(unsigned long xl, long y)
}
#endif
#if 0 // unused
static void recalc_climb_rate()
{
float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2);
climb_rate = (int)(slope*100);
}
static void print_climb_debug_info()
{
unsigned char i, j;
recalc_climb_rate();
//SendDebugln_P("Climb rate:");
for (i=0; i<ALTITUDE_HISTORY_LENGTH; i++) {
j = i + hindex;
if (j >= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH;
//SendDebug_P(" ");
//SendDebug(j,DEC);
//SendDebug_P(": ");
//SendDebug(history[j].x,DEC);
//SendDebug_P(", ");
//SendDebugln(history[j].y,DEC);
}
//SendDebug_P(" sum(xi) = ");
//SendDebugln(xi,DEC);
//SendDebug_P(" sum(yi) = ");
//SendDebugln(yi,DEC);
//SendDebug_P(" sum(xi*yi) = ");
//SendDebugln(xiyi,DEC);
//SendDebug_P(" sum(xi^2) = ");
//SendDebugln(xi2,DEC);
//SendDebug_P(" Climb rate = ");
//SendDebug((float)climb_rate/100,2);
//SendDebugln_P(" m/s");
}
#endif