Adding fabs and explicit type conversion

[joezie]

I found two bugs in navigation-related source codes. Details are listed below:

• nav_smooth.c (the earliest version with the bug: tree e2ccf1b in release v5.17_devel)
  

  paparazzi/sw/airborne/modules/nav/nav_smooth.c

  Lines 157 to 170 in e2ccf1b

  	static void compute_ground_speed(float airspeed,
  	float wind_x,
  	float wind_y) {
  	uint8_t i;
  	float alpha = 0;
  	float c = wind_x*wind_x+wind_y*wind_y-airspeed*airspeed;
  	for( i=0; i < NB_ANGLES; i++, alpha+=ANGLE_STEP) {
  	/* g^2 -2 scal g + c = 0 */
  	float scal = wind_x*cos(alpha) + wind_y*sin(alpha);
  	float delta = 4 * (scal*scal - c);
  	ground_speeds[i] = scal + sqrt(delta)/2.;
  	Bound(ground_speeds[i], NOMINAL_AIRSPEED/4, 2*NOMINAL_AIRSPEED);
  	}
  	}

  
  delta is impacted by the values of wind_east, wind_north, and airspeed. In some cases, the computed delta can be a negative value, and then we perform sqrt() on it, which result to NaN error. To reproduce this error, you can manually set stateGetHorizontalWindspeed_f()->y and stateGetHorizontalWindspeed_f()->x to -10.0 and 10.0 respectively at the following function call and run flight simulation.
  

  paparazzi/sw/airborne/modules/nav/nav_smooth.c

  Line 189 in e2ccf1b

  compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y, stateGetHorizontalWindspeed_f()->x); // Wind in NED frame

  
  To fix this bug, a possible solution is to perform fabs() on delta before performing sqrt().
  ground_speeds[i] = scal + sqrt(fabs(delta))/2.;
  But please note that the computed result might not be the ideal value.

• navigation.c (the earliest version with the bug: tree dd0c6cc in release v5.17_devel)
  

  paparazzi/sw/airborne/firmwares/rotorcraft/navigation.c

  Line 274 in dd0c6cc

  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;

  
  Here, wp_diff and pos_diff are two-dimensional variables representing the horizontal distance
  between two waypoints wp_start and wp_end and between the UAV and wp_start respectively.
  The types of their members x and y are both signed integers. However, since nav_leg_length
  is an unsigned integer, the result computed within the parentheses must be implicitly converted
  to unsigned integer before divided by nav_leg_length. When the UAV is approaching the waypoint
  wp_start, if the result computed within the parentheses happens to be a negative value whose most significant bit is set to 1, it would be interpreted as a large value after conversion to unsigned
  integer. After being divided by nav_leg_length, the most significant bit becomes 0, and therefore,
  the final result is converted back to a very large signed integer and assigned to nav_leg_progress.
  

  paparazzi/sw/airborne/firmwares/rotorcraft/navigation.c

  Line 278 in dd0c6cc

  Bound(nav_leg_progress, 0, prog_2);

  
  Later, nav_leg_progress is bounded within the range [0, prog_2], and consequently be trimmed to the upper bound prog_2. This value is not reasonable because nav_leg_progress reflects the navigation progress, so it should not have shown a completed progress before straight navigation begins. To reproduce this error, you can set the start and end waypoint in a relatively short distance (so that nav_leg_length is relatively small), and have the UAV fly towards the start waypoint from a direction that could make the computation result of (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) to be a negative value.
  To fix this bug, we can add explicit type conversion to nav_leg_length:
  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / (int32_t)nav_leg_length;
  Even the computation result within the parentheses is a negative value and the computed nav_leg_progress is consequently a negative value, it would then be bounded within the range [0, prog_2], and consequently be trimmed to the lower bound 0. This would be a much reasonable value to show a starting progress of navigation.

[noether]

Hi @joezie . Thanks a lot for spotting these two issues.

The second fix looks alright to me.

As you pointed out, I am not sure about the first fix. I think the first issue should be fixed by checking the values of the airspeed or the wind speeds (in case a sensor does not give an admissible value?) to guarantee that delta is positive for all the alphas. On the other hand, I did not write this part, and I would need some time to see the whole picture/algorithm.

What do you think @gautierhattenberger ?

[gautierhattenberger]

I also agree for the second point.

The first algo actually makes no sense if your airspeed is lower that the wind speed, since you would never reach the point.
So, I agree that checking for consistent inputs would be better, but using abs is not the right way I think.

[noether]

Yeah, it seems that we can check if airspeed => windspeed, so we have always delta => 0 (no idea now whether there are corner cases because of the operations with floats).

The question now would be to think about how to handle the "exception" if airspeed < windspeed.

[gautierhattenberger]

Since this navigation function is not able to handle this case anyway, I would just "quit" (return false) and let the flight plan handle the rest.

[joezie]

I agree with the idea to return false on "exception".