SITL: Change division to multiplication

This commit is contained in:
muramura 2024-09-27 12:59:50 +10:00 committed by Peter Barker
parent 8d990d56e5
commit 637aec0085
7 changed files with 14 additions and 14 deletions

View File

@ -68,16 +68,16 @@ void Gripper_EPM::update_from_demand()
const float dt = (now - last_update_us) * 1.0e-6f; const float dt = (now - last_update_us) * 1.0e-6f;
// decay the field // 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 // note that "demand" here is just an on/off switch; we only care
// about which range it falls into // about which range it falls into
if (demand > 0.6f) { if (demand > 0.6f) {
// we are instructed to grip harder // 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) { } else if (demand < 0.4f) {
// we are instructed to loosen grip // 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 { } else {
// neutral; no demanded change // neutral; no demanded change
} }
@ -121,5 +121,5 @@ float Gripper_EPM::tesla() const
// https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field) // https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field)
// 200N lifting capacity ~= 2.5T // 200N lifting capacity ~= 2.5T
const float percentage_to_tesla = 0.25f; 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);
} }

View File

@ -89,7 +89,7 @@ void Gripper_Servo::update(const struct sitl_input &input)
position_demand = position; 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); position = constrain_float(position_demand, position - position_max_change, position + position_max_change);
float jaw_gap; float jaw_gap;
if ((release_pwm < grab_pwm && reverse) || (release_pwm > grab_pwm && !reverse)) { if ((release_pwm < grab_pwm && reverse) || (release_pwm > grab_pwm && !reverse)) {

View File

@ -55,5 +55,5 @@ uint32_t RF_LightWareSerial::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, ui
alt_cm = 13000; // from datasheet 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
} }

View File

@ -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 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) // 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; uint8_t checksum = 0;
for (uint8_t i=1; i<ret; i++) { // 1 because the initial $ is skipped for (uint8_t i=1; i<ret; i++) { // 1 because the initial $ is skipped
checksum ^= buffer[i]; checksum ^= buffer[i];

View File

@ -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) 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);
} }

View File

@ -76,7 +76,7 @@ void Sprayer::update(const struct sitl_input &input)
if (pump_demand < 0) { // never updated if (pump_demand < 0) { // never updated
pump_demand = 0; 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 = last_pump_output =
constrain_float(pump_demand, last_pump_output - pump_max_change, last_pump_output + pump_max_change); 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); last_pump_output = constrain_float(last_pump_output, 0, 1);

View File

@ -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); post_location.offset(x*10+3, y*10+2);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (postfile != nullptr) { 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 #endif
Vector2f post_position_cm; 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 CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (intersectionsfile != nullptr) { if (intersectionsfile != nullptr) {
Location intersection_point = location; Location intersection_point = location;
intersection_point.offset(intersection_point_cm.x/100.0, intersection_point.offset(intersection_point_cm.x*0.01,
intersection_point_cm.y/100.0); intersection_point_cm.y*0.01);
::fprintf(intersectionsfile, ::fprintf(intersectionsfile,
"map icon %f %f barrell\n", "map icon %f %f barrell\n",
intersection_point.lat*1e-7, intersection_point.lat*1e-7,
@ -1792,8 +1792,8 @@ float SIM::measure_distance_at_angle_bf(const Location &location, float angle) c
} }
#endif #endif
// ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm/100.0f); // ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm*0.01f);
return min_dist_cm / 100.0f; return min_dist_cm * 0.01f;
} }
} // namespace SITL } // namespace SITL