zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

APMRover2: Use c++ cast

mission-4.1.18
Pierre Kancir 8 years ago committed by Grant Morphett
parent
1a246851fc
commit
b634fe548d
12 changed files with 76 additions and 79 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      APMrover2/APMrover2.cpp
  2. 17
      APMrover2/GCS_Mavlink.cpp
  3. 24
      APMrover2/Log.cpp
  4. 4
      APMrover2/Parameters.cpp
  5. 2
      APMrover2/Steering.cpp
  6. 24
      APMrover2/commands_logic.cpp
  7. 2
      APMrover2/control_modes.cpp
  8. 2
      APMrover2/failsafe.cpp
  9. 2
      APMrover2/radio.cpp
  10. 14
      APMrover2/sensors.cpp
  11. 4
      APMrover2/system.cpp
  12. 58
      APMrover2/test.cpp

2
APMrover2/APMrover2.cpp
Unescape View File

@ -338,7 +338,7 @@ void Rover::one_second_loop(void) @@ -338,7 +338,7 @@ void Rover::one_second_loop(void)
// write perf data every 20s
if (counter % 10 == 0) {
if (scheduler.debug() != 0) {
hal.console->printf("G_Dt_max=%lu\n", (unsigned long)G_Dt_max);
hal.console->printf("G_Dt_max=%lu\n", static_cast<uint64_t>(G_Dt_max));
}
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();

17
APMrover2/GCS_Mavlink.cpp
Unescape View File

@ -100,7 +100,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan) @@ -100,7 +100,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
(uint16_t)(scheduler.load_average(20000) * 1000),
static_cast<uint16_t>(scheduler.load_average(20000) * 1000), // WRONG float to uint16_t
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
@ -179,8 +179,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan) @@ -179,8 +179,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan)
gps.ground_speed(),
ahrs.groundspeed(),
(ahrs.yaw_sensor / 100) % 360,
(uint16_t)(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))),
current_loc.alt / 100.0,
static_cast<uint16_t>(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))), // WRONG float to uint16
0);
}
@ -281,7 +280,7 @@ void Rover::send_current_waypoint(mavlink_channel_t chan) @@ -281,7 +280,7 @@ void Rover::send_current_waypoint(mavlink_channel_t chan)
uint32_t GCS_MAVLINK_Rover::telem_delay() const
{
return (uint32_t)(rover.g.telem_delay);
return static_cast<uint32_t>(rover.g.telem_delay);
}
// try to send a message, return false if it won't fit in the serial tx buffer
@ -948,7 +947,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) @@ -948,7 +947,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
}
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
switch (static_cast<uint16_t>(packet.param1)) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
rover.set_mode(MANUAL);
@ -1068,9 +1067,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) @@ -1068,9 +1067,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
GCS_MAVLINK::send_home_all(new_home_loc);
result = MAV_RESULT_ACCEPTED;
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat * 1.0e-7f),
(double)(new_home_loc.lng * 1.0e-7f),
(uint32_t)(new_home_loc.alt * 0.01f));
static_cast<double>(new_home_loc.lat * 1.0e-7f),
static_cast<double>(new_home_loc.lng * 1.0e-7f),
static_cast<uint32_t>(new_home_loc.alt * 0.01f)); // WRONG FLoat to uint32
}
break;
}
@ -1588,7 +1587,7 @@ void Rover::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...) @@ -1588,7 +1587,7 @@ void Rover::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
char str[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
va_list arg_list;
va_start(arg_list, fmt);
hal.util->vsnprintf((char *)str, sizeof(str), fmt, arg_list);
hal.util->vsnprintf(reinterpret_cast<char *>(str), sizeof(str), fmt, arg_list);
va_end(arg_list);
gcs().send_statustext(severity, 0xFF, str);
notify.send_text(str);

24
APMrover2/Log.cpp
Unescape View File

@ -72,13 +72,13 @@ int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv) @@ -72,13 +72,13 @@ int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv)
cliSerial->printf("dumping all\n");
Log_Read(0, 1, 0);
return(-1);
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
} else if ((argc != 2) || (static_cast<uint16_t>(dump_log_num) > DataFlash.get_num_logs())) {
cliSerial->printf("bad log number\n");
return(-1);
}
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
Log_Read(static_cast<uint16_t>(dump_log_num), dump_log_start, dump_log_end);
return 0;
}
@ -266,11 +266,11 @@ void Rover::Log_Write_Nav_Tuning() @@ -266,11 +266,11 @@ void Rover::Log_Write_Nav_Tuning()
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_us : AP_HAL::micros64(),
yaw : (uint16_t)ahrs.yaw_sensor,
yaw : static_cast<uint16_t>(ahrs.yaw_sensor), // int32 to uint16
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
throttle : (int8_t)(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle)),
target_bearing_cd : static_cast<uint16_t>(nav_controller->target_bearing_cd()), // int32 to uint16
nav_bearing_cd : static_cast<uint16_t>(nav_controller->nav_bearing_cd()), // int32 to uint16
throttle : static_cast<int8_t>((100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle))), // wrong float to int8
xtrack_error : nav_controller->crosstrack_error()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -322,13 +322,13 @@ void Rover::Log_Write_Sonar() @@ -322,13 +322,13 @@ void Rover::Log_Write_Sonar()
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : lateral_acceleration,
sonar1_distance : (uint16_t)sonar.distance_cm(0),
sonar2_distance : (uint16_t)sonar.distance_cm(1),
sonar1_distance : sonar.distance_cm(0),
sonar2_distance : sonar.distance_cm(1),
detected_count : obstacle.detected_count,
turn_angle : (int8_t)obstacle.turn_angle,
turn_angle : static_cast<int8_t>(obstacle.turn_angle), // WRONG float to int8
turn_time : turn_time,
ground_speed : (uint16_t)(ground_speed*100),
throttle : (int8_t)(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle))
ground_speed : static_cast<uint16_t>(ground_speed * 100), // WRONT float to uint16
throttle : static_cast<int8_t>((100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle))) //WRONG float to int8
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -486,7 +486,7 @@ void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page @@ -486,7 +486,7 @@ void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page
{
cliSerial->printf("\n" FIRMWARE_STRING
"\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
static_cast<uint32_t>(hal.util->available_memory()));
cliSerial->printf("%s\n", HAL_BOARD_NAME);

4
APMrover2/Parameters.cpp
Unescape View File

@ -617,9 +617,7 @@ void Rover::load_parameters(void) @@ -617,9 +617,7 @@ void Rover::load_parameters(void)
Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
const uint16_t old_aux_chan_mask = 0x3FFA;
SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap);
cliSerial->printf("load_all took %luus\n", micros() - before);
cliSerial->printf("load_all took %luus\n", static_cast<uint64_t>(micros() - before));
// set a more reasonable default NAVL1_PERIOD for rovers
L1_controller.set_default_period(NAVL1_PERIOD);
}

2
APMrover2/Steering.cpp
Unescape View File

@ -56,7 +56,7 @@ bool Rover::auto_check_trigger(void) { @@ -56,7 +56,7 @@ bool Rover::auto_check_trigger(void) {
if (!is_zero(g.auto_kickstart)) {
float xaccel = ins.get_accel().x;
if (xaccel >= g.auto_kickstart) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", (double)xaccel);
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
auto_triggered = true;
return true;
}

24
APMrover2/commands_logic.cpp
Unescape View File

@ -260,8 +260,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -260,8 +260,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// Check if this is the first time we have reached the waypoint
if (!previously_reached_wp) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
(unsigned)cmd.index,
(unsigned)loiter_duration);
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(loiter_duration));
// record the current time i.e. start timer
loiter_start_time = millis();
previously_reached_wp = true;
@ -275,8 +275,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -275,8 +275,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
}
} else {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Distance %um",
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP));
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(get_distance(current_loc, next_WP))); // WRONG float to uint32
}
// set loiter_duration to 0 so we know we aren't or have finished loitering
loiter_duration = 0;
@ -295,8 +295,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -295,8 +295,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// Check if this is the first time we have reached the waypoint even though we have gone past it
if (!previously_reached_wp) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
(unsigned)cmd.index,
(unsigned)loiter_duration);
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(loiter_duration));
// record the current time i.e. start timer
loiter_start_time = millis();
previously_reached_wp = true;
@ -308,8 +308,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -308,8 +308,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
if ((uint32_t)distance_past_wp != (uint32_t)dist_to_wp) {
distance_past_wp = dist_to_wp;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i. Distance %um",
(unsigned)cmd.index,
(unsigned)distance_past_wp);
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(distance_past_wp)); // WRONG float to uint32
}
// Check if we need to loiter at this waypoint
@ -337,7 +337,7 @@ bool Rover::verify_RTL() @@ -337,7 +337,7 @@ bool Rover::verify_RTL()
// have we gone past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached destination. Distance away %um",
(unsigned)get_distance(current_loc, next_WP));
static_cast<uint32_t>(get_distance(current_loc, next_WP))); // WRONG float to uint32
rtl_complete = true;
return true;
}
@ -396,7 +396,7 @@ void Rover::nav_set_yaw_speed() @@ -396,7 +396,7 @@ void Rover::nav_set_yaw_speed()
void Rover::do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
condition_value = static_cast<int32_t>(cmd.content.delay.seconds * 1000); // convert seconds to milliseconds
}
void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd)
@ -410,7 +410,7 @@ void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd) @@ -410,7 +410,7 @@ void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd)
bool Rover::verify_wait_delay()
{
if ((uint32_t)(millis() - condition_start) > (uint32_t)condition_value) {
if (static_cast<uint32_t>(millis() - condition_start) > static_cast<uint32_t>(condition_value)) {
condition_value = 0;
return true;
}
@ -434,7 +434,7 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd) @@ -434,7 +434,7 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
g.speed_cruise.set(cmd.content.speed.target_ms);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", (double)g.speed_cruise.get());
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", static_cast<double>(g.speed_cruise.get()));
}
if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {

2
APMrover2/control_modes.cpp
Unescape View File

@ -118,7 +118,7 @@ void Rover::read_trim_switch() @@ -118,7 +118,7 @@ void Rover::read_trim_switch()
// save command
if (mission.add_cmd(cmd)) {
hal.console->printf("Learning waypoint %u", (unsigned) mission.num_commands());
hal.console->printf("Learning waypoint %u", static_cast<uint32_t>(mission.num_commands()));
}
break;
}

2
APMrover2/failsafe.cpp
Unescape View File

@ -38,7 +38,7 @@ void Rover::failsafe_check() @@ -38,7 +38,7 @@ void Rover::failsafe_check()
}
if (in_failsafe && tnow - last_timestamp > 20000 &&
channel_throttle->read() >= (uint16_t)g.fs_throttle_value) {
channel_throttle->read() >= static_cast<uint16_t>(g.fs_throttle_value)) {
// pass RC inputs to outputs every 20ms
last_timestamp = tnow;
hal.rcin->clear_overrides();

2
APMrover2/radio.cpp
Unescape View File

@ -188,7 +188,7 @@ void Rover::control_failsafe(uint16_t pwm) @@ -188,7 +188,7 @@ void Rover::control_failsafe(uint16_t pwm)
if (rc_override_active) {
failsafe_trigger(FAILSAFE_EVENT_RC, (millis() - failsafe.rc_override_timer) > 1500);
} else if (g.fs_throttle_enabled) {
bool failed = pwm < (uint16_t)g.fs_throttle_value;
bool failed = pwm < static_cast<uint16_t>(g.fs_throttle_value);
if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 2000) {
failed = true;
}

14
APMrover2/sensors.cpp
Unescape View File

@ -65,26 +65,26 @@ void Rover::read_sonars(void) @@ -65,26 +65,26 @@ void Rover::read_sonars(void)
// we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = sonar.distance_cm(1);
if (obstacle.sonar1_distance_cm < (uint16_t)g.sonar_trigger_cm &&
obstacle.sonar1_distance_cm < (uint16_t)obstacle.sonar2_distance_cm) {
if (obstacle.sonar1_distance_cm < static_cast<uint16_t>(g.sonar_trigger_cm) &&
obstacle.sonar1_distance_cm < static_cast<uint16_t>(obstacle.sonar2_distance_cm)) {
// we have an object on the left
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm",
(unsigned)obstacle.sonar1_distance_cm);
static_cast<uint32_t>(obstacle.sonar1_distance_cm));
}
obstacle.detected_time_ms = AP_HAL::millis();
obstacle.turn_angle = g.sonar_turn_angle;
} else if (obstacle.sonar2_distance_cm < (uint16_t)g.sonar_trigger_cm) {
} else if (obstacle.sonar2_distance_cm < static_cast<uint16_t>(g.sonar_trigger_cm)) {
// we have an object on the right
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm",
(unsigned)obstacle.sonar2_distance_cm);
static_cast<uint32_t>(obstacle.sonar2_distance_cm));
}
obstacle.detected_time_ms = AP_HAL::millis();
obstacle.turn_angle = -g.sonar_turn_angle;
@ -93,14 +93,14 @@ void Rover::read_sonars(void) @@ -93,14 +93,14 @@ void Rover::read_sonars(void)
// we have a single sonar
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm < (uint16_t)g.sonar_trigger_cm) {
if (obstacle.sonar1_distance_cm < static_cast<uint16_t>(g.sonar_trigger_cm)) {
// obstacle detected in front
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar obstacle %u cm",
(unsigned)obstacle.sonar1_distance_cm);
static_cast<uint32_t>(obstacle.sonar1_distance_cm));
}
obstacle.detected_time_ms = AP_HAL::millis();
obstacle.turn_angle = g.sonar_turn_angle;

4
APMrover2/system.cpp
Unescape View File

@ -387,7 +387,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) @@ -387,7 +387,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
control_mode != RTL &&
control_mode != HOLD) {
failsafe.triggered = failsafe.bits;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", (unsigned)failsafe.triggered);
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
switch (g.fs_action) {
case 0:
break;
@ -467,7 +467,7 @@ void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode) @@ -467,7 +467,7 @@ void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
port->printf("RTL");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
port->printf("Mode(%u)", static_cast<uint32_t>(mode));
break;
}
}

58
APMrover2/test.cpp
Unescape View File

@ -146,8 +146,8 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv) @@ -146,8 +146,8 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
cliSerial->printf("%u waypoints\n", (unsigned)mission.num_commands());
cliSerial->printf("Hit radius: %f\n", (double)g.waypoint_radius.get());
cliSerial->printf("%u waypoints\n", static_cast<uint32_t>(mission.num_commands()));
cliSerial->printf("Hit radius: %f\n", static_cast<double>(g.waypoint_radius.get()));
for (uint8_t i = 0; i < mission.num_commands(); i++) {
AP_Mission::Mission_Command temp_cmd;
@ -162,13 +162,13 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv) @@ -162,13 +162,13 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv)
void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd)
{
cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n",
(int)cmd.index,
(int)cmd.id,
(int)cmd.content.location.options,
(int)cmd.p1,
(long)cmd.content.location.alt,
(long)cmd.content.location.lat,
(long)cmd.content.location.lng);
static_cast<int32_t>(cmd.index),
static_cast<int32_t>(cmd.id),
static_cast<int32_t>(cmd.content.location.options),
static_cast<int32_t>(cmd.p1),
static_cast<int64_t>(cmd.content.location.alt),
static_cast<int64_t>(cmd.content.location.lat),
static_cast<int64_t>(cmd.content.location.lng));
}
int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
@ -222,10 +222,10 @@ int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv) @@ -222,10 +222,10 @@ int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
last_message_time_ms = gps.last_message_time_ms();
const Location &loc = gps.location();
cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n",
(long)loc.lat,
(long)loc.lng,
(long)loc.alt/100,
(int)gps.num_sats());
static_cast<int64_t>(loc.lat),
static_cast<int64_t>(loc.lng),
static_cast<int64_t>(loc.alt/100),
static_cast<int32_t>(gps.num_sats()));
} else {
cliSerial->printf(".");
}
@ -267,11 +267,11 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv) @@ -267,11 +267,11 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
Vector3f gyros = ins.get_gyro();
Vector3f accels = ins.get_accel();
cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n",
(int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100,
(double)gyros.x, (double)gyros.y, (double)gyros.z,
(double)accels.x, (double)accels.y, (double)accels.z);
static_cast<int32_t>(ahrs.roll_sensor / 100),
static_cast<int32_t>(ahrs.pitch_sensor / 100),
static_cast<uint16_t>(ahrs.yaw_sensor / 100),
static_cast<double>(gyros.x), static_cast<double>(gyros.y), static_cast<double>(gyros.z),
static_cast<double>(accels.x), static_cast<double>(accels.y), static_cast<double>(accels.z));
if (cliSerial->available() > 0) {
return (0);
}
@ -336,9 +336,9 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) @@ -336,9 +336,9 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
const Vector3f mag_ofs = compass.get_offsets();
const Vector3f mag = compass.get_field();
cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(double)(wrap_360_cd(ToDeg(heading) * 100)) /100,
(double)mag.x, (double)mag.y, (double)mag.z,
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
static_cast<double>((wrap_360_cd(ToDeg(heading) * 100)) /100),
static_cast<double>(mag.x), static_cast<double>(mag.y), static_cast<double>(mag.z),
static_cast<double>(mag_ofs.x), static_cast<double>(mag_ofs.y), static_cast<double>(mag_ofs.z));
} else {
cliSerial->printf("compass not healthy\n");
}
@ -408,14 +408,14 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv) @@ -408,14 +408,14 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv)
if (now - last_print >= 200) {
cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n",
(double)sonar_dist_cm_min,
(double)sonar_dist_cm_max,
(double)voltage_min,
(double)voltage_max,
(double)sonar2_dist_cm_min,
(double)sonar2_dist_cm_max,
(double)voltage2_min,
(double)voltage2_max);
static_cast<double>(sonar_dist_cm_min),
static_cast<double>(sonar_dist_cm_max),
static_cast<double>(voltage_min),
static_cast<double>(voltage_max),
static_cast<double>(sonar2_dist_cm_min),
static_cast<double>(sonar2_dist_cm_max),
static_cast<double>(voltage2_min),
static_cast<double>(voltage2_max));
voltage_min = voltage_max = 0.0f;
voltage2_min = voltage2_max = 0.0f;
sonar_dist_cm_min = sonar_dist_cm_max = 0.0f;

Write Preview
Loading…
Cancel
Save