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

AP_Math: switch get_distance_cm to return uint32_t

Browse Source 
Includes changes required on ArduCopter and ArduPlane side as well
This commit is contained in:
Randy Mackay 2013-01-26 17:04:12 +09:00
parent 0056bfadd7
commit d2767b911c
8 changed files with 22 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -699,7 +699,7 @@ static int32_t home_bearing;
static int32_t home_distance;
// distance between plane and next_WP in cm
// is not static because AP_Camera uses it
int32_t wp_distance;
uint32_t wp_distance;
////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors

20
ArduCopter/commands_logic.pde
View File

@ -391,14 +391,10 @@ static bool verify_nav_wp()
}
// Did we pass the WP? // Distance checking
if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) {
// if we have a distance calc error, wp_distance may be less than 0
if(wp_distance > 0) {
wp_verify_byte |= NAV_LOCATION;
if(loiter_time == 0) {
loiter_time = millis();
}
if((wp_distance <= (unsigned long)max((g.waypoint_radius * 100),0)) || check_missed_wp()) {
wp_verify_byte |= NAV_LOCATION;
if(loiter_time == 0) {
loiter_time = millis();
}
}
@ -428,7 +424,7 @@ static bool verify_nav_wp()
static bool verify_loiter_unlimited()
{
if(nav_mode == NAV_WP && wp_distance <= (g.waypoint_radius * 100)) {
if(nav_mode == NAV_WP && wp_distance <= (unsigned long)max((g.waypoint_radius * 100),0)) {
// switch to position hold
set_nav_mode(NAV_LOITER);
}
@ -443,7 +439,7 @@ static bool verify_loiter_time()
return true;
}
}
if(nav_mode == NAV_WP && wp_distance <= (g.waypoint_radius * 100)) {
if(nav_mode == NAV_WP && wp_distance <= (unsigned long)max((g.waypoint_radius * 100),0)) {
// reset our loiter time
loiter_time = millis();
// switch to position hold
@ -499,7 +495,7 @@ static bool verify_RTL()
case RTL_STATE_RETURNING_HOME:
// if we've reached home initiate loiter
if (wp_distance <= g.waypoint_radius * 100 || check_missed_wp()) {
if (wp_distance <= (unsigned long)max((g.waypoint_radius * 100),0) || check_missed_wp()) {
rtl_state = RTL_STATE_LOITERING_AT_HOME;
set_nav_mode(NAV_LOITER);
@ -653,7 +649,7 @@ static bool verify_change_alt()
static bool verify_within_distance()
{
//cliSerial->printf("cond dist :%d\n", (int)condition_value);
if (wp_distance < condition_value) {
if (wp_distance < max(condition_value,0)) {
condition_value = 0;
return true;
}

2
ArduCopter/navigation.pde
View File

@ -565,7 +565,7 @@ static void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (wp_distance >= (g.crosstrack_min_distance * 100) &&
if (wp_distance >= (unsigned long)max((g.crosstrack_min_distance * 100),0) &&
abs(wrap_180(wp_bearing - original_wp_bearing)) < 4500) {
float temp = (wp_bearing - original_wp_bearing) * RADX100;

2
ArduPlane/ArduPlane.pde
View File

@ -498,7 +498,7 @@ static int32_t nav_pitch_cd;
////////////////////////////////////////////////////////////////////////////////
// Distance between plane and next waypoint. Meters
// is not static because AP_Camera uses it
int32_t wp_distance;
uint32_t wp_distance;
// Distance between previous and next waypoint. Meters
static int32_t wp_totalDistance;

8
ArduPlane/commands_logic.pde
View File

@ -318,7 +318,7 @@ static bool verify_land()
// Set land_complete if we are within 2 seconds distance or within
// 3 meters altitude of the landing point
if (((wp_distance > 0) && (wp_distance <= (g.land_flare_sec*g_gps->ground_speed*0.01)))
if ((wp_distance <= (g.land_flare_sec*g_gps->ground_speed*0.01))
|| (adjusted_altitude_cm() <= next_WP.alt + g.land_flare_alt*100)) {
land_complete = true;
@ -359,7 +359,7 @@ static bool verify_nav_wp()
{
hold_course = -1;
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
if (wp_distance <= (unsigned)max(g.waypoint_radius,0)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(&current_loc, &next_WP));
@ -416,7 +416,7 @@ static bool verify_loiter_turns()
static bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
if (wp_distance <= (unsigned)max(g.waypoint_radius,0)) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
}else{
@ -477,7 +477,7 @@ static bool verify_change_alt()
static bool verify_within_distance()
{
if (wp_distance < condition_value) {
if (wp_distance < max(condition_value,0)) {
condition_value = 0;
return true;
}

6
ArduPlane/navigation.pde
View File

@ -148,11 +148,11 @@ static void update_loiter()
{
float power;
if(wp_distance <= g.loiter_radius) {
if(wp_distance <= (unsigned)max(g.loiter_radius,0)) {
power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
nav_bearing_cd += 9000.0 * (2.0 + power);
} else if(wp_distance < (g.loiter_radius + LOITER_RANGE)) {
} else if(wp_distance < (unsigned long)max((g.loiter_radius + LOITER_RANGE),0)) {
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing_cd -= power * 9000;
@ -191,7 +191,7 @@ static void update_crosstrack(void)
// Crosstrack Error
// ----------------
// If we are too far off or too close we don't do track following
if (wp_totalDistance >= g.crosstrack_min_distance &&
if (wp_totalDistance >= (unsigned)max(g.crosstrack_min_distance,0) &&
abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
// Meters we are off track line
crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;

2
libraries/AP_Math/AP_Math.h
View File

@ -51,7 +51,7 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2,
float get_distance(const struct Location *loc1, const struct Location *loc2);
// return distance in centimeters between two locations
int32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2);
uint32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2);
// return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2);

12
libraries/AP_Math/location.cpp
View File

@ -43,22 +43,16 @@ static float longitude_scale(const struct Location *loc)
// return distance in meters to between two locations, or -1
// if one of the locations is invalid
// return distance in meters to between two locations
float get_distance(const struct Location *loc1, const struct Location *loc2)
{
if (loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2);
return pythagorous2(dlat, dlong) * 0.01113195f;
}
// return distance in centimeters to between two locations, or -1 if
// one of the locations is invalid
int32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2)
// return distance in centimeters to between two locations
uint32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2)
{
return get_distance(loc1, loc2) * 100;
}