void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 == 1) { set_reverse(true); } else { set_reverse(false); } }
void Rover::set_mode(enum mode mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. return; } // If we are changing out of AUTO mode reset the loiter timer if (control_mode == AUTO) loiter_time = 0; control_mode = mode; throttle_last = 0; throttle = 500; set_reverse(false); g.pidSpeedThrottle.reset_I(); if (control_mode != AUTO) { auto_triggered = false; } switch(control_mode) { case MANUAL: case HOLD: case LEARNING: case STEERING: break; case AUTO: rtl_complete = false; restart_nav(); break; case RTL: do_RTL(); break; case GUIDED: rtl_complete = false; /* when entering guided mode we set the target as the current location. This matches the behaviour of the copter code. */ guided_WP = current_loc; set_guided_WP(); break; default: do_RTL(); break; } if (should_log(MASK_LOG_MODE)) { DataFlash.Log_Write_Mode(control_mode); } }
Locomotive::Locomotive(int pin_a, int pin_b, int pin_pwm) : _pin_a(pin_a), _pin_b(pin_b), _pin_pwm(pin_pwm), _speed(0), _reverse(false) { pinMode(_pin_a, OUTPUT); pinMode(_pin_b, OUTPUT); pinMode(_pin_pwm, OUTPUT); set_reverse(false); set_speed(0.0); }
void Rover::set_mode(enum mode mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. return; } control_mode = mode; throttle_last = 0; throttle = 500; set_reverse(false); g.pidSpeedThrottle.reset_I(); if (control_mode != AUTO) { auto_triggered = false; } switch(control_mode) { case MANUAL: case HOLD: case LEARNING: case STEERING: break; case AUTO: rtl_complete = false; restart_nav(); break; case RTL: do_RTL(); break; case GUIDED: rtl_complete = false; set_guided_WP(); break; default: do_RTL(); break; } if (should_log(MASK_LOG_MODE)) { DataFlash.Log_Write_Mode(control_mode); } }
void Rover::update_current_mode(void) { switch (control_mode){ case AUTO: case RTL: set_reverse(false); calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); break; case GUIDED: set_reverse(false); if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are if (channel_throttle->servo_out != g.throttle_min.get()) { gcs_send_mission_item_reached_message(0); } channel_throttle->servo_out = g.throttle_min.get(); channel_steer->servo_out = 0; lateral_acceleration = 0; } else { calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); } break; case STEERING: { /* in steering mode we control lateral acceleration directly. We first calculate the maximum lateral acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn. We get the radius of turn from half the STEER2SRV_P. */ float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius(); // constrain to user set TURN_MAX_G max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS); lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f); calc_nav_steer(); // and throttle gives speed in proportion to cruise speed, up // to 50% throttle, then uses nudging above that. float target_speed = channel_throttle->pwm_to_angle() * 0.01f * 2 * g.speed_cruise; set_reverse(target_speed < 0); if (in_reverse) { target_speed = constrain_float(target_speed, -g.speed_cruise, 0); } else { target_speed = constrain_float(target_speed, 0, g.speed_cruise); } calc_throttle(target_speed); break; } case LEARNING: case MANUAL: /* 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 */ channel_throttle->servo_out = channel_throttle->control_in; channel_steer->servo_out = channel_steer->pwm_to_angle(); // mark us as in_reverse when using a negative throttle to // stop AHRS getting off set_reverse(channel_throttle->servo_out < 0); break; case HOLD: // hold position - stop motors and center steering channel_throttle->servo_out = 0; channel_steer->servo_out = 0; set_reverse(false); break; case INITIALISING: break; } }
/* calculate the throtte for auto-throttle modes */ void Rover::calc_throttle(float target_speed) { // If not autostarting OR we are loitering at a waypoint // then set the throttle to minimum if (!auto_check_trigger() || in_stationary_loiter()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); // Stop rotation in case of loitering and skid steering if (g.skid_steer_out) { SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); } return; } const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise; const int throttle_target = throttle_base + throttle_nudge; /* reduce target speed in proportion to turning rate, up to the SPEED_TURN_GAIN percentage. */ float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS)); steer_rate = constrain_float(steer_rate, 0.0f, 1.0f); // use g.speed_turn_gain for a 90 degree turn, and in proportion // for other turn angles const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor); const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1); const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f; float reduction = 1.0f - steer_rate * speed_turn_reduction; if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) { // in auto-modes we reduce speed when approaching waypoints const float reduction2 = 1.0f - speed_turn_reduction; if (reduction2 < reduction) { reduction = reduction2; } } // reduce the target speed by the reduction factor target_speed *= reduction; groundspeed_error = fabsf(target_speed) - ground_speed; throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100); // also reduce the throttle by the reduction factor. This gives a // much faster response in turns throttle *= reduction; if (in_reverse) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); } else { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(throttle, g.throttle_min, g.throttle_max)); } if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) { // the user has asked to use reverse throttle to brake. Apply // it in proportion to the ground speed error, but only when // our ground speed error is more than BRAKING_SPEEDERR. // // We use a linear gain, with 0 gain at a ground speed error // of braking_speederr, and 100% gain when groundspeed_error // is 2*braking_speederr const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1); const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain; SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); // temporarily set us in reverse to allow the PWM setting to // go negative set_reverse(true); } if (use_pivot_steering()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); } }
void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd) { mode_auto.set_reversed(cmd.p1 == 1); set_reverse(cmd.p1 == 1); }
void Rover::update_current_mode(void) { switch (control_mode) { case AUTO: case RTL: if (!in_auto_reverse) { set_reverse(false); } if (!do_auto_rotation) { calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); } else { do_yaw_rotation(); } break; case GUIDED: { if (!in_auto_reverse) { set_reverse(false); } switch (guided_mode) { case Guided_Angle: nav_set_yaw_speed(); break; case Guided_WP: if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are 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); lateral_acceleration = 0; } else { calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt), Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0)); } break; default: gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); break; } break; } case STEERING: { /* in steering mode we control lateral acceleration directly. We first calculate the maximum lateral acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn. We get the radius of turn from half the STEER2SRV_P. */ float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius(); // constrain to user set TURN_MAX_G max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS); lateral_acceleration = max_g_force * (channel_steer->get_control_in()/4500.0f); calc_nav_steer(); // and throttle gives speed in proportion to cruise speed, up // to 50% throttle, then uses nudging above that. float target_speed = channel_throttle->get_control_in() * 0.01f * 2 * g.speed_cruise; set_reverse(target_speed < 0); if (in_reverse) { target_speed = constrain_float(target_speed, -g.speed_cruise, 0); } else { target_speed = constrain_float(target_speed, 0, g.speed_cruise); } calc_throttle(target_speed); break; } case LEARNING: case MANUAL: /* 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 */ 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 set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0); break; 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); if (!in_auto_reverse) { set_reverse(false); } break; case INITIALISING: break; } }