// 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();
}
