diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index ebb6822436..bd518fcd91 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -102,7 +102,7 @@ void Rover::setup() AP_Param::setup_sketch_defaults(); in_auto_reverse = false; - + rssi.init(); init_ardupilot(); @@ -436,7 +436,7 @@ void Rover::update_GPS_10Hz(void) void Rover::update_current_mode(void) { - switch (control_mode){ + switch (control_mode) { case AUTO: case RTL: if (!in_auto_reverse) { @@ -451,7 +451,7 @@ void Rover::update_current_mode(void) if (!in_auto_reverse) { set_reverse(false); } - switch (guided_mode){ + switch (guided_mode) { case Guided_Angle: nav_set_yaw_speed(); break; @@ -462,8 +462,8 @@ void Rover::update_current_mode(void) if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) { gcs_send_mission_item_reached_message(0); } - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); lateral_acceleration = 0; } else { calc_lateral_acceleration(); @@ -518,9 +518,8 @@ void Rover::update_current_mode(void) we set the exact value in set_servos(), but it helps for logging */ - in_auto_reverse = false; - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,channel_steer->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in()); // mark us as in_reverse when using a negative throttle to // stop AHRS getting off @@ -529,8 +528,8 @@ void Rover::update_current_mode(void) case HOLD: // hold position - stop motors and center steering - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); if (!in_auto_reverse) { set_reverse(false); } @@ -560,13 +559,13 @@ void Rover::update_navigation() calc_lateral_acceleration(); calc_nav_steer(); if (verify_RTL()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); set_mode(HOLD); } break; case GUIDED: - switch (guided_mode){ + switch (guided_mode) { case Guided_Angle: nav_set_yaw_speed(); break; @@ -577,8 +576,8 @@ void Rover::update_navigation() calc_nav_steer(); if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); lateral_acceleration = 0; } break; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index af9c4cce5c..31494912ba 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -180,6 +180,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan) ahrs.groundspeed(), (ahrs.yaw_sensor / 100) % 360, static_cast(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))), // WRONG float to uint16 + current_loc.alt / 100.0f, 0); } @@ -195,7 +196,7 @@ void Rover::send_hwstatus(mavlink_channel_t chan) { mavlink_msg_hwstatus_send( chan, - hal.analogin->board_voltage()*1000, + hal.analogin->board_voltage() * 1000, 0); } @@ -978,7 +979,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_REPEAT_SERVO: - if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { + if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4 * 1000)) { result = MAV_RESULT_ACCEPTED; } break; @@ -990,7 +991,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_REPEAT_RELAY: - if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { + if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3 * 1000)) { result = MAV_RESULT_ACCEPTED; } break; diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 2c0fc4542e..b7a1fec40f 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -7,7 +7,6 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK { - public: void data_stream_send(void) override; @@ -23,5 +22,4 @@ private: bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; bool try_send_message(enum ap_message id) override; - }; diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 613097a525..21835ab1cb 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -279,7 +279,7 @@ void Rover::Log_Write_Nav_Tuning() // Write an attitude packet void Rover::Log_Write_Attitude() { - Vector3f targets(0, 0, 0); // Rover does not have attitude targets, use place-holder for commonality with Dataflash Log_Write_Attitude message + Vector3f targets(0.0f, 0.0f, 0.0f); // Rover does not have attitude targets, use place-holder for commonality with Dataflash Log_Write_Attitude message DataFlash.Log_Write_Attitude(ahrs, targets); diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 45924e9fc5..00d8ba7180 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -599,15 +599,15 @@ void Rover::load_parameters(void) cliSerial->printf("done.\n"); } - unsigned long before = micros(); + uint32_t before = micros(); // Load all auto-loaded EEPROM variables AP_Param::load_all(); AP_Param::set_frame_type_flags(AP_PARAM_FRAME_ROVER); - + SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering); SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle); - + const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old, Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old, Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old, diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 3e2964e5f1..5fefd4f544 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -288,7 +288,7 @@ public: Parameters() : // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- - pidSpeedThrottle (0.7, 0.2, 0.2, 4000) + pidSpeedThrottle (0.7f, 0.2f, 0.2f, 4000) {} }; @@ -310,7 +310,7 @@ public: // RC input channels RC_Channels rc_channels; - + // control over servo output ranges SRV_Channels servo_channels; @@ -318,7 +318,6 @@ public: // advanced failsafe library AP_AdvancedFailsafe_Rover afs; #endif - }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 910500705f..09be7add75 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -49,6 +49,6 @@ Rover::Rover(void) : frsky_telemetry(ahrs, battery, sonar), #endif home(ahrs.get_home()), - G_Dt(0.02) + G_Dt(0.02f) { } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6a9bd821c4..d29adf2669 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -186,7 +186,7 @@ private: AP_SerialManager serial_manager; const uint8_t num_gcs; GCS_MAVLINK_Rover gcs_chan[MAVLINK_COMM_NUM_BUFFERS]; - GCS _gcs; // avoid using this; use gcs() + GCS _gcs; // avoid using this; use gcs() GCS &gcs() { return _gcs; } // relay support diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 88eee6a9ea..9789b9ee54 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -172,7 +172,7 @@ void Rover::calc_throttle(float target_speed) { } if (use_pivot_steering()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); } } diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index d519f01140..52d80a2b79 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -305,7 +305,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // check if distance to the WP has changed and output new message if it has float dist_to_wp = get_distance(current_loc, next_WP); - if ((uint32_t)distance_past_wp != (uint32_t)dist_to_wp) { + if (!is_equal(distance_past_wp, dist_to_wp)) { distance_past_wp = dist_to_wp; gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i. Distance %um", static_cast(cmd.index), @@ -370,14 +370,14 @@ void Rover::nav_set_yaw_speed() // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt if ((millis() - guided_yaw_speed.msg_time_ms) > 3000) { gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping"); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); lateral_acceleration = 0; return; } int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,steering); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering); // speed param in the message gives speed as a proportion of cruise speed. // 0.5 would set speed to the cruise speed @@ -401,7 +401,7 @@ void Rover::do_wait_delay(const AP_Mission::Mission_Command& cmd) void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd) { - condition_value = cmd.content.distance.meters; + condition_value = cmd.content.distance.meters; } /********************************************************************************/ @@ -432,12 +432,12 @@ bool Rover::verify_within_distance() void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd) { - if (cmd.content.speed.target_ms > 0) { + if (cmd.content.speed.target_ms > 0.0f) { g.speed_cruise.set(cmd.content.speed.target_ms); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", static_cast(g.speed_cruise.get())); } - if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { + if (cmd.content.speed.throttle_pct > 0.0f && cmd.content.speed.throttle_pct <= 100.0f) { g.throttle_cruise.set(cmd.content.speed.throttle_pct); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", g.throttle_cruise.get()); } diff --git a/APMrover2/navigation.cpp b/APMrover2/navigation.cpp index f9d92f4c37..9a8d4c84a4 100644 --- a/APMrover2/navigation.cpp +++ b/APMrover2/navigation.cpp @@ -11,7 +11,7 @@ void Rover::navigate() return; } - if ((next_WP.lat == 0 && next_WP.lng == 0) || (home_is_set == HOME_UNSET)){ + if ((next_WP.lat == 0 && next_WP.lng == 0) || (home_is_set == HOME_UNSET)) { return; } diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 2f3437436b..16093810b3 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -18,9 +18,9 @@ void Rover::set_control_channels(void) // For a rover safety is TRIM throttle if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim()); + hal.rcout->set_safety_pwm(1UL << (rcmap.throttle() - 1), channel_throttle->get_radio_trim()); if (g.skid_steer_out) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1), channel_steer->get_radio_trim()); + hal.rcout->set_safety_pwm(1UL << (rcmap.roll() - 1), channel_steer->get_radio_trim()); } } // setup correct scaling for ESCs like the UAVCAN PX4ESC which @@ -50,9 +50,9 @@ void Rover::init_rc_out() // For Rover's no throttle means TRIM as rovers can go backwards i.e. MIN throttle is // full speed backward. if (arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim()); + hal.rcout->set_safety_pwm(1UL << (rcmap.throttle() - 1), channel_throttle->get_radio_trim()); if (g.skid_steer_out) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1), channel_steer->get_radio_trim()); + hal.rcout->set_safety_pwm(1UL << (rcmap.roll() - 1), channel_steer->get_radio_trim()); } } } @@ -131,7 +131,7 @@ void Rover::read_radio() control_failsafe(channel_throttle->get_radio_in()); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); // Check if the throttle value is above 50% and we need to nudge // Make sure its above 50% in the direction we are travelling @@ -139,7 +139,7 @@ void Rover::read_radio() (((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) && in_reverse) || ((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) && !in_reverse))) { throttle_nudge = (g.throttle_max - g.throttle_cruise) * - ((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f); + ((fabsf(channel_throttle->norm_input()) - 0.5f) / 0.5f); } else { throttle_nudge = 0; } @@ -157,18 +157,19 @@ void Rover::read_radio() float motor1 = channel_steer->norm_input(); float motor2 = channel_throttle->norm_input(); float steering_scaled = motor1 - motor2; - float throttle_scaled = 0.5f*(motor1 + motor2); + float throttle_scaled = 0.5f * (motor1 + motor2); + int16_t steer = channel_steer->get_radio_trim(); int16_t thr = channel_throttle->get_radio_trim(); if (steering_scaled > 0.0f) { - steer += steering_scaled*(channel_steer->get_radio_max()-channel_steer->get_radio_trim()); + steer += steering_scaled * (channel_steer->get_radio_max()-channel_steer->get_radio_trim()); } else { - steer += steering_scaled*(channel_steer->get_radio_trim()-channel_steer->get_radio_min()); + steer += steering_scaled * (channel_steer->get_radio_trim()-channel_steer->get_radio_min()); } if (throttle_scaled > 0.0f) { - thr += throttle_scaled*(channel_throttle->get_radio_max()-channel_throttle->get_radio_trim()); + thr += throttle_scaled * (channel_throttle->get_radio_max()-channel_throttle->get_radio_trim()); } else { - thr += throttle_scaled*(channel_throttle->get_radio_trim()-channel_throttle->get_radio_min()); + thr += throttle_scaled * (channel_throttle->get_radio_trim()-channel_throttle->get_radio_min()); } channel_steer->set_pwm(steer); channel_throttle->set_pwm(thr); diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 1bb9767d54..9e3573bba6 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -268,7 +268,7 @@ void Rover::set_reverse(bool reverse) void Rover::set_mode(enum mode mode) { - if (control_mode == mode){ + if (control_mode == mode) { // don't switch modes if we are already in the correct mode. return; } diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index 8668119c3b..3532c86735 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -58,7 +58,7 @@ int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv) } cliSerial->printf("\n"); } - if (cliSerial->available() > 0){ + if (cliSerial->available() > 0) { return (0); } } @@ -96,7 +96,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) fail_test++; } - if (oldSwitchPosition != readSwitch()){ + if (oldSwitchPosition != readSwitch()) { cliSerial->printf("CONTROL MODE CHANGED: "); print_mode(cliSerial, readSwitch()); cliSerial->printf("\n"); @@ -183,7 +183,7 @@ int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv) while (1) { delay(20); uint8_t switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ + if (oldSwitchPosition != switchPosition) { cliSerial->printf("Position %d\n", switchPosition); oldSwitchPosition = switchPosition; }