mirror of https://github.com/ArduPilot/ardupilot
SITL: Change division to multiplication
This commit is contained in:
parent
8d990d56e5
commit
637aec0085
|
@ -68,16 +68,16 @@ void Gripper_EPM::update_from_demand()
|
|||
const float dt = (now - last_update_us) * 1.0e-6f;
|
||||
|
||||
// decay the field
|
||||
field_strength = field_strength * (100.0f - field_decay_rate * dt) / 100.0f;
|
||||
field_strength = field_strength * (100.0f - field_decay_rate * dt) * 0.01f;
|
||||
|
||||
// note that "demand" here is just an on/off switch; we only care
|
||||
// about which range it falls into
|
||||
if (demand > 0.6f) {
|
||||
// we are instructed to grip harder
|
||||
field_strength = field_strength + (100.0f - field_strength) * field_strength_slew_rate / 100.0f * dt;
|
||||
field_strength = field_strength + (100.0f - field_strength) * field_strength_slew_rate * 0.01f * dt;
|
||||
} else if (demand < 0.4f) {
|
||||
// we are instructed to loosen grip
|
||||
field_strength = field_strength * (100.0f - field_degauss_rate * dt) / 100.0f;
|
||||
field_strength = field_strength * (100.0f - field_degauss_rate * dt) * 0.01f;
|
||||
} else {
|
||||
// neutral; no demanded change
|
||||
}
|
||||
|
@ -121,5 +121,5 @@ float Gripper_EPM::tesla() const
|
|||
// https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field)
|
||||
// 200N lifting capacity ~= 2.5T
|
||||
const float percentage_to_tesla = 0.25f;
|
||||
return static_cast<float>(percentage_to_tesla * field_strength / 100.0f);
|
||||
return static_cast<float>(percentage_to_tesla * field_strength * 0.01f);
|
||||
}
|
||||
|
|
|
@ -89,7 +89,7 @@ void Gripper_Servo::update(const struct sitl_input &input)
|
|||
position_demand = position;
|
||||
}
|
||||
|
||||
const float position_max_change = position_slew_rate / 100.0f * dt;
|
||||
const float position_max_change = position_slew_rate * 0.01f * dt;
|
||||
position = constrain_float(position_demand, position - position_max_change, position + position_max_change);
|
||||
float jaw_gap;
|
||||
if ((release_pwm < grab_pwm && reverse) || (release_pwm > grab_pwm && !reverse)) {
|
||||
|
|
|
@ -55,5 +55,5 @@ uint32_t RF_LightWareSerial::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, ui
|
|||
alt_cm = 13000; // from datasheet
|
||||
}
|
||||
|
||||
return snprintf((char*)buffer, buflen, "%0.2f\r", alt_cm / 100.0f); // note tragic lack of snprintf return checking
|
||||
return snprintf((char*)buffer, buflen, "%0.2f\r", alt_cm * 0.01f); // note tragic lack of snprintf return checking
|
||||
}
|
||||
|
|
|
@ -30,7 +30,7 @@ uint32_t RF_NMEA::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t bufle
|
|||
// Format 2 DBT NMEA mode (e.g. $SMDBT,5.94,f,1.81,M,67)
|
||||
// Format 3 DPT NMEA mode (e.g. $SMDPT,1.81,0.066)
|
||||
|
||||
ssize_t ret = snprintf((char*)buffer, buflen, "$SMDPT,%f,%f", alt_cm/100.0f, 0.01f);
|
||||
ssize_t ret = snprintf((char*)buffer, buflen, "$SMDPT,%f,%f", alt_cm*0.01f, 0.01f);
|
||||
uint8_t checksum = 0;
|
||||
for (uint8_t i=1; i<ret; i++) { // 1 because the initial $ is skipped
|
||||
checksum ^= buffer[i];
|
||||
|
|
|
@ -113,5 +113,5 @@ void RF_Wasp::update(float range)
|
|||
|
||||
uint32_t RF_Wasp::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
|
||||
{
|
||||
return snprintf((char*)buffer, buflen, "%f\n", alt_cm/100.0f);
|
||||
return snprintf((char*)buffer, buflen, "%f\n", alt_cm*0.01f);
|
||||
}
|
||||
|
|
|
@ -76,7 +76,7 @@ void Sprayer::update(const struct sitl_input &input)
|
|||
if (pump_demand < 0) { // never updated
|
||||
pump_demand = 0;
|
||||
}
|
||||
const float pump_max_change = pump_slew_rate / 100.0f * dt;
|
||||
const float pump_max_change = pump_slew_rate * 0.01f * dt;
|
||||
last_pump_output =
|
||||
constrain_float(pump_demand, last_pump_output - pump_max_change, last_pump_output + pump_max_change);
|
||||
last_pump_output = constrain_float(last_pump_output, 0, 1);
|
||||
|
|
|
@ -1755,7 +1755,7 @@ float SIM::measure_distance_at_angle_bf(const Location &location, float angle) c
|
|||
post_location.offset(x*10+3, y*10+2);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (postfile != nullptr) {
|
||||
::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm/100.0);
|
||||
::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm*0.01);
|
||||
}
|
||||
#endif
|
||||
Vector2f post_position_cm;
|
||||
|
@ -1769,8 +1769,8 @@ float SIM::measure_distance_at_angle_bf(const Location &location, float angle) c
|
|||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (intersectionsfile != nullptr) {
|
||||
Location intersection_point = location;
|
||||
intersection_point.offset(intersection_point_cm.x/100.0,
|
||||
intersection_point_cm.y/100.0);
|
||||
intersection_point.offset(intersection_point_cm.x*0.01,
|
||||
intersection_point_cm.y*0.01);
|
||||
::fprintf(intersectionsfile,
|
||||
"map icon %f %f barrell\n",
|
||||
intersection_point.lat*1e-7,
|
||||
|
@ -1792,8 +1792,8 @@ float SIM::measure_distance_at_angle_bf(const Location &location, float angle) c
|
|||
}
|
||||
#endif
|
||||
|
||||
// ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm/100.0f);
|
||||
return min_dist_cm / 100.0f;
|
||||
// ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm*0.01f);
|
||||
return min_dist_cm * 0.01f;
|
||||
}
|
||||
|
||||
} // namespace SITL
|
||||
|
|
Loading…
Reference in New Issue