diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index fdd9019612..0be4edaefc 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -390,8 +390,7 @@ static int16_t throttle_last = 0, throttle = 500; //////////////////////////////////////////////////////////////////////////////// // Difference between current bearing and desired bearing. in centi-degrees static int32_t bearing_error_cd; -// Difference between current altitude and desired altitude. Centimeters -static int32_t altitude_error; + // Distance perpandicular to the course line that we are off trackline. Meters static float crosstrack_error; @@ -422,13 +421,13 @@ static float current_total1; //////////////////////////////////////////////////////////////////////////////// // Navigation control variables //////////////////////////////////////////////////////////////////////////////// -// The instantaneous desired bank angle. Hundredths of a degree +// The instantaneous desired steering angle. Hundredths of a degree static int32_t nav_steer; //////////////////////////////////////////////////////////////////////////////// // Waypoint distances //////////////////////////////////////////////////////////////////////////////// -// Distance between plane and next waypoint. Meters +// Distance between rover and next waypoint. Meters static float wp_distance; // Distance between previous and next waypoint. Meters static int32_t wp_totalDistance; @@ -471,7 +470,7 @@ static struct Location home; static bool home_is_set; // The location of the previous waypoint. Used for track following and altitude ramp calculations static struct Location prev_WP; -// The plane's current location +// The rover's current location static struct Location current_loc; // The location of the current/active waypoint. Used for track following static struct Location next_WP; @@ -656,11 +655,6 @@ static void fast_loop() // --------------------------------------- update_current_mode(); - // apply desired steering if in an auto mode - if (control_mode >= AUTO) { - g.channel_steer.servo_out = nav_steer; - } - // write out the servo PWM values // ------------------------------ set_servos(); @@ -847,12 +841,32 @@ static void update_current_mode(void) case RTL: case GUIDED: calc_nav_steer(); - calc_throttle(); + calc_throttle(g.speed_cruise); + break; + + case STEERING: + /* + in steering mode we control the bearing error, which gives + the same type of steering control as auto mode. The throttle + controls the target speed, in proportion to the throttle + */ + bearing_error_cd = g.channel_steer.pwm_to_angle(); + calc_nav_steer(); + + /* we need to reset the I term or it will build up */ + g.pidNavSteer.reset_I(); + calc_throttle(g.channel_throttle.pwm_to_angle() * 0.01 * g.speed_cruise); break; case LEARNING: case MANUAL: - nav_steer = 0; + /* + in both MANUAL and LEARNING we pass through the + controls. Setting servo_out here actually doesn't matter, as + we set the exact value in set_servos(), but it helps for + logging + */ + g.channel_throttle.servo_out = g.channel_throttle.radio_in; g.channel_steer.servo_out = g.channel_steer.pwm_to_angle(); break; @@ -866,6 +880,7 @@ static void update_navigation() switch (control_mode) { case MANUAL: case LEARNING: + case STEERING: case INITIALISING: break; diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 38c9b3964a..76c943b427 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -42,9 +42,8 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) // ArduPlane documentation switch (control_mode) { case MANUAL: - base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; case LEARNING: + case STEERING: base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case AUTO: @@ -139,6 +138,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack break; case LEARNING: + case STEERING: control_sensors_enabled |= (1<<10); // 3D angular rate control control_sensors_enabled |= (1<<11); // attitude stabilisation break; @@ -244,7 +244,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) bearing, target_bearing / 100, wp_distance, - altitude_error / 1.0e2, + 0, groundspeed_error, crosstrack_error); } @@ -1082,6 +1082,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (packet.custom_mode) { case MANUAL: case LEARNING: + case STEERING: case AUTO: case RTL: set_mode((enum mode)packet.custom_mode); diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index d8eb29bbda..e2161add22 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -420,10 +420,9 @@ static void Log_Read_Control_Tuning() struct log_Nav_Tuning { LOG_PACKET_HEADER; uint16_t yaw; - uint32_t wp_distance; + float wp_distance; uint16_t target_bearing_cd; uint16_t nav_bearing_cd; - int16_t altitude_error_cm; int16_t nav_gain_scheduler; }; @@ -433,10 +432,9 @@ static void Log_Write_Nav_Tuning() struct log_Nav_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG), yaw : (uint16_t)ahrs.yaw_sensor, - wp_distance : (uint32_t)wp_distance, + wp_distance : wp_distance, target_bearing_cd : (uint16_t)target_bearing, nav_bearing_cd : (uint16_t)nav_bearing, - altitude_error_cm : (int16_t)altitude_error, nav_gain_scheduler : (int16_t)(nav_gain_scaler*1000) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -448,12 +446,11 @@ static void Log_Read_Nav_Tuning() struct log_Nav_Tuning pkt; DataFlash.ReadPacket(&pkt, sizeof(pkt)); - cliSerial->printf_P(PSTR("NTUN, %4.4f, %lu, %4.4f, %4.4f, %4.4f, %4.4f\n"), + cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f\n"), (float)pkt.yaw/100.0f, - (unsigned long)pkt.wp_distance, + pkt.wp_distance, (float)(pkt.target_bearing_cd/100.0f), (float)(pkt.nav_bearing_cd/100.0f), - (float)(pkt.altitude_error_cm/100.0f), (float)(pkt.nav_gain_scheduler/100.0f)); } diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index ec48d9d76e..d5adf405ba 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -268,7 +268,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @DisplayName: Sonar trigger angle // @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the sonar. A positive number means to turn right, and a negative angle means to turn left. // @Units: centimeters - // @Range: -90 90 + // @Range: -45 45 // @Increment: 1 // @User: Standard GSCALAR(sonar_turn_angle, "SONAR_TURN_ANGLE", 45), @@ -290,7 +290,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: MODE1 // @DisplayName: Mode1 - // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided // @User: Standard // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049) GSCALAR(mode1, "MODE1", MODE_1), @@ -298,35 +298,35 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: MODE2 // @DisplayName: Mode2 // @Description: Driving mode for switch position 2 (1231 to 1360) - // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode2, "MODE2", MODE_2), // @Param: MODE3 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode3, "MODE3", MODE_3), // @Param: MODE4 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode4, "MODE4", MODE_4), // @Param: MODE5 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode5, "MODE5", MODE_5), // @Param: MODE6 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,2:LEARNING,3:STEERING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode6, "MODE6", MODE_6), diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 5eab21c188..dfa8c4a412 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -17,11 +17,35 @@ static void throttle_slew_limit(int16_t last_throttle) } } -static void calc_throttle() +static void calc_throttle(float target_speed) { + if (target_speed <= 0) { + // cope with zero requested speed + g.channel_throttle.servo_out = g.throttle_min.get(); + return; + } + int throttle_target = g.throttle_cruise + throttle_nudge; + + /* + reduce target speed in proportion to turning rate, up to the + SPEED_TURN_GAIN percentage. + */ + float steer_rate = fabsf(nav_steer / (float)SERVO_MAX); + steer_rate = constrain(steer_rate, 0.0, 1.0); + float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01; + + if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) { + // in auto-modes we reduce speed when approaching waypoints + float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist); + if (reduction2 < reduction) { + reduction = reduction2; + } + } + + target_speed *= reduction; - groundspeed_error = g.speed_cruise - ground_speed; + groundspeed_error = target_speed - ground_speed; throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100); g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get()); @@ -45,8 +69,10 @@ static void calc_nav_steer() nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler); if (obstacle) { // obstacle avoidance - nav_steer += g.sonar_turn_angle; + nav_steer += g.sonar_turn_angle*100; } + + g.channel_steer.servo_out = nav_steer; } /***************************************** @@ -56,23 +82,15 @@ static void set_servos(void) { int16_t last_throttle = g.channel_throttle.radio_out; - if ((control_mode == MANUAL) || (control_mode == LEARNING)) { + if (control_mode == MANUAL || control_mode == LEARNING) { // do a direct pass through of radio values g.channel_steer.radio_out = g.channel_steer.radio_in; - - if (obstacle) // obstacle in front, turn right in Stabilize mode - g.channel_steer.radio_out -= 500; - g.channel_throttle.radio_out = g.channel_throttle.radio_in; } else { g.channel_steer.calc_pwm(); - g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); - } - - if (control_mode >= AUTO) { // convert 0 to 100% into PWM g.channel_throttle.calc_pwm(); diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index 0934c23530..1d0d84ef4d 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -79,7 +79,7 @@ static void read_trim_switch() } CH7_wp_index = 1; return; - } else if (control_mode == LEARNING) { + } else if (control_mode == LEARNING || control_mode == STEERING) { // if SW7 is ON in LEARNING = record the Wp // set the next_WP (home is stored at 0) diff --git a/APMrover2/defines.h b/APMrover2/defines.h index a4b148b6fd..7454454994 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -70,6 +70,7 @@ enum ch7_option { enum mode { MANUAL=0, LEARNING=2, + STEERING=3, AUTO=10, RTL=11, GUIDED=15, diff --git a/APMrover2/events.pde b/APMrover2/events.pde index 9dc3246dbd..da8416959f 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -11,6 +11,7 @@ static void failsafe_long_on_event(int fstype) { case MANUAL: case LEARNING: + case STEERING: set_mode(RTL); break; diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index 2c8b6f793a..8d05a12b56 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -15,7 +15,7 @@ static void navigate() return; } - // waypoint distance from plane + // waypoint distance from rover // ---------------------------- wp_distance = get_distance(¤t_loc, &next_WP); diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index c9da745bdd..b228f44007 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -2,7 +2,7 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the rover circling static void init_rc_in() diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index a7d5b0151e..dbe8b9b7f5 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -231,6 +231,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) while ( mode != MANUAL && mode != LEARNING && + mode != STEERING && mode != AUTO && mode != RTL) { diff --git a/APMrover2/system.pde b/APMrover2/system.pde index bc6dce7732..5a4fa1705e 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -311,6 +311,7 @@ static void set_mode(enum mode mode) { case MANUAL: case LEARNING: + case STEERING: break; case AUTO: @@ -362,7 +363,7 @@ static void startup_INS_ground(bool force_accel_level) // Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!! // ----------------------- demo_servos(2); - gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane")); + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move vehicle")); mavlink_delay(1000); ahrs.init(); @@ -493,6 +494,9 @@ print_mode(uint8_t mode) case LEARNING: cliSerial->println_P(PSTR("Learning")); break; + case STEERING: + cliSerial->println_P(PSTR("Stearing")); + break; case AUTO: cliSerial->println_P(PSTR("AUTO")); break;