// update navigation void AR_WPNav::update(float dt) { // exit immediately if no current location, origin or destination Location current_loc; float speed; if (!hal.util->get_soft_armed() || !_orig_and_dest_valid || !AP::ahrs().get_position(current_loc) || !_atc.get_forward_speed(speed)) { _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); _desired_turn_rate_rads = 0.0f; return; } // if no recent calls initialise desired_speed_limited to current speed if (!is_active()) { _desired_speed_limited = speed; } _last_update_ms = AP_HAL::millis(); update_distance_and_bearing_to_destination(); // check if vehicle has reached the destination const bool near_wp = _distance_to_destination <= _radius; const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination); if (!_reached_destination && (near_wp || past_wp)) { _reached_destination = true; } // calculate the required turn of the wheels update_steering(current_loc, speed); // calculate desired speed update_desired_speed(dt); }
bool AP_Landing_Deepstall::override_servos(void) { if (!(stage == DEEPSTALL_STAGE_LAND)) { return false; } SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator); if (elevator == nullptr) { // deepstalls are impossible without these channels, abort the process GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels"); request_go_around(); return false; } // calculate the progress on slewing the elevator float slew_progress = 1.0f; if (slew_speed > 0) { slew_progress = (AP_HAL::millis() - stall_entry_time) / (100.0f * slew_speed); slew_progress = constrain_float (slew_progress, 0.0f, 1.0f); } // mix the elevator to the correct value elevator->set_output_pwm(linear_interpolate(initial_elevator_pwm, elevator_pwm, slew_progress, 0.0f, 1.0f)); // use the current airspeed to dictate the travel limits float airspeed; landing.ahrs.airspeed_estimate(&airspeed); // only allow the deepstall steering controller to run below the handoff airspeed if (slew_progress >= 1.0f || airspeed <= handoff_airspeed) { // run the steering conntroller float pid = update_steering(); float travel_limit = constrain_float((handoff_airspeed - airspeed) / (handoff_airspeed - handoff_lower_limit_airspeed) * 0.5f + 0.5f, 0.5f, 1.0f); float output = constrain_float(pid, -travel_limit, travel_limit); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, output*4500); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, output*4500); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, output*4500); } else { // allow the normal servo control of the channel SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)); } // hand off rudder control to deepstall controlled return true; }
void steering_calibration(){ esc.detach(); SERIAL_OUT.println(); angle_target = 0.0; steering.attach(STEERING); steering.writeMicroseconds(STEER_ADJUST); delay(500); setup_mpu6050(); calculate_null(); SERIAL_OUT.println("set controller to automatic"); get_mode(); while(mode != AUTOMATIC) get_mode(); accum = 0; //this is ONLY used to reset the 0 the gyro angle for real (setting angle to 0 does nothing!!! (never forget last year's debacle)) delay(250); esc.attach(THROTTLE); delay(1000); esc.writeMicroseconds(S_STOP); delay(1000); esc.writeMicroseconds(S_LOW); while(mode == AUTOMATIC){ read_FIFO(); update_steering(); steering.writeMicroseconds(steer_us); if((millis() - time) > 500){ SERIAL_OUT.print("angle: "); SERIAL_OUT.print((angle*180.0/3.1415),5); SERIAL_OUT.print("\tsteering ms: "); SERIAL_OUT.println(steer_us); time = millis(); } get_mode(); } steering.detach(); esc.detach(); return ; }
/** * To be able to follow the line * * @param curr_error The error from the line */ void drive(int8_t curr_error) { // Update steering with new PD value update_steering(pd_update(curr_error), STEERING_MAX_SPEED); enable_timer_interrupts(); }