Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: formatting fixes and const local variables

Browse Source
This commit is contained in:
khancyr 2017-08-03 11:53:22 +02:00 committed by Randy Mackay
parent 848df551ac
commit 7c3374668e
3 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
APMrover2/Log.cpp
View File

@ -467,9 +467,9 @@ void Rover::Log_Write_WheelEncoder()
LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG), LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
distance_0 : g2.wheel_encoder.get_distance(0), distance_0 : g2.wheel_encoder.get_distance(0),
quality_0 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(0),0.0f,100.0f), quality_0 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(0), 0.0f, 100.0f),
distance_1 : g2.wheel_encoder.get_distance(1), distance_1 : g2.wheel_encoder.get_distance(1),
quality_1 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(1),0.0f,100.0f) quality_1 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(1), 0.0f, 100.0f)
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -503,7 +503,6 @@ void Rover::log_init(void)
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
gcs().reset_cli_timeout(); gcs().reset_cli_timeout();
} }
#if CLI_ENABLED == ENABLED #if CLI_ENABLED == ENABLED

2
APMrover2/radio.cpp
View File

@ -129,7 +129,7 @@ void Rover::read_radio()
const float left_input = channel_steer->norm_input(); const float left_input = channel_steer->norm_input();
const float right_input = channel_throttle->norm_input(); const float right_input = channel_throttle->norm_input();
const float throttle_scaled = 0.5f * (left_input + right_input); const float throttle_scaled = 0.5f * (left_input + right_input);
float steering_scaled = constrain_float(left_input - right_input,-1.0f,1.0f); float steering_scaled = constrain_float(left_input - right_input, -1.0f, 1.0f);
// flip the steering direction if requesting the vehicle reverse (to be consistent with separate steering-throttle frames) // flip the steering direction if requesting the vehicle reverse (to be consistent with separate steering-throttle frames)
if (is_negative(throttle_scaled)) { if (is_negative(throttle_scaled)) {

18
APMrover2/sensors.cpp
View File

@ -104,10 +104,10 @@ void Rover::update_wheel_encoder()
g2.wheel_encoder.update(); g2.wheel_encoder.update();
// initialise on first iteration // initialise on first iteration
uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (wheel_encoder_last_ekf_update_ms == 0) { if (wheel_encoder_last_ekf_update_ms == 0) {
wheel_encoder_last_ekf_update_ms = now; wheel_encoder_last_ekf_update_ms = now;
for (uint8_t i = 0; i<g2.wheel_encoder.num_sensors(); i++) { for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
wheel_encoder_last_angle_rad[i] = g2.wheel_encoder.get_delta_angle(i); wheel_encoder_last_angle_rad[i] = g2.wheel_encoder.get_delta_angle(i);
wheel_encoder_last_update_ms[i] = g2.wheel_encoder.get_last_reading_ms(i); wheel_encoder_last_update_ms[i] = g2.wheel_encoder.get_last_reading_ms(i);
} }
@ -119,18 +119,17 @@ void Rover::update_wheel_encoder()
// send delta time (time between this wheel encoder time and previous wheel encoder time) // send delta time (time between this wheel encoder time and previous wheel encoder time)
// in case where wheel hasn't moved, count = 0 (cap the delta time at 50ms - or system time) // in case where wheel hasn't moved, count = 0 (cap the delta time at 50ms - or system time)
// use system clock of last update instead of time of last ping // use system clock of last update instead of time of last ping
float system_dt = (now - wheel_encoder_last_ekf_update_ms) / 1000.0f; const float system_dt = (now - wheel_encoder_last_ekf_update_ms) / 1000.0f;
for (uint8_t i = 0; i<g2.wheel_encoder.num_sensors(); i++) { for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
// calculate angular change (in radians) // calculate angular change (in radians)
float curr_angle_rad = g2.wheel_encoder.get_delta_angle(i); const float curr_angle_rad = g2.wheel_encoder.get_delta_angle(i);
float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i]; const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i];
wheel_encoder_last_angle_rad[i] = curr_angle_rad; wheel_encoder_last_angle_rad[i] = curr_angle_rad;
// calculate delta time // calculate delta time
float delta_time; float delta_time;
uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i); const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i);
uint32_t sensor_diff_ms = latest_sensor_update_ms - wheel_encoder_last_update_ms[i]; const uint32_t sensor_diff_ms = latest_sensor_update_ms - wheel_encoder_last_update_ms[i];
// if we have not received any sensor updates, or time difference is too high then use time since last update to the ekf // if we have not received any sensor updates, or time difference is too high then use time since last update to the ekf
// check for old or insane sensor update times // check for old or insane sensor update times
@ -155,7 +154,6 @@ void Rover::update_wheel_encoder()
} else { } else {
wheel_encoder_rpm[i] = 0.0f; wheel_encoder_rpm[i] = 0.0f;
} }
} }
// record system time update for next iteration // record system time update for next iteration