ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source
http://ardupilot.org/
GNU General Public License v3.0
10.81k stars 17.27k forks source link

Copter: divide by zero exceptions caused by parameter being set to zero #15709

Open khancyr opened 3 years ago

khancyr commented 3 years ago

Bug report

Issue details

See https://discuss.ardupilot.org/t/arithmetic-exception/63684 for some parameters that could lead to FPE on division by zero

Version What version was the issue encountered with

Platform [ ] All [ ] AntennaTracker [ X ] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type What type of airframe (flying wing, glider, hex, Y6, octa etc)

Hardware type What autopilot hardware was used? (Pixhawk, Cube, Pixracer, Navio2, etc)

Logs Please provide a link to any relevant logs that show the issue

KimHyungSub commented 3 years ago

I would like to suggest patches. I have confirmed these patches prevent the divide by zero and integer overflow cases. But, you guys can optimize my suggestions. Thanks!

int32_t GCS_MAVLINK::global_position_int_relative_alt() const { float posD; AP::ahrs().get_relative_position_D_home(posD);

if (((posD > 0) && (posD > INT32_MAX/1000.0f)) || ((posD < 0) && (posD < INT32_MIN/1000.0f))) {
    return posD;
}
else {
    posD *= -1000.0f; // change from down to up and metres to millimeters
}

return posD;

}

void Location::offset(float ofs_north, float ofs_east) { // use is_equal() because is_zero() is a local class conflict and is_zero() in AP_Math does not belong to a class if (!is_equal(ofs_north, 0.0f) || !is_equal(ofs_east, 0.0f)) {

    if (((ofs_north > 0) && (ofs_north > INT32_MAX/LOCATION_SCALING_FACTOR_INV)) || ((ofs_north < 0) && (ofs_north < INT32_MIN/LOCATION_SCALING_FACTOR_INV))) {
        return;
    }

    int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
    int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
    lat += dlat;
    lng += dlng;
}

}

int16_t RC_Channel::get_control_mid() const { if (type_in == RC_CHANNEL_TYPE_RANGE) { int16_t r_in = (radio_min.get() + radio_max.get())/2;

    if (reversed) {
        r_in = radio_max.get() - (r_in - radio_min.get());
    }

    int16_t radio_trim_low  = radio_min + dead_zone;

    if ((int32_t)(radio_max - radio_trim_low) == 0) {
        return 0;
    }

    return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));
} else {
    return 0;
}

}