mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
676d75c391
This makes us look like Rover and Plane in terms of namespacing for the Mode classes, and removes a wart where we #include mode.h in the middle of the Mode class. This was done mechanically for the most part. I've had to remove the convenience reference for ap as part of this.
94 lines
3.3 KiB
C++
94 lines
3.3 KiB
C++
#include "Copter.h"
|
|
|
|
#if MODE_CIRCLE_ENABLED == ENABLED
|
|
|
|
/*
|
|
* Init and run calls for circle flight mode
|
|
*/
|
|
|
|
// circle_init - initialise circle controller flight mode
|
|
bool ModeCircle::init(bool ignore_checks)
|
|
{
|
|
pilot_yaw_override = false;
|
|
|
|
// initialize speeds and accelerations
|
|
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
|
|
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
|
|
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
|
|
pos_control->set_max_accel_z(g.pilot_accel_z);
|
|
|
|
// initialise circle controller including setting the circle center based on vehicle speed
|
|
copter.circle_nav->init();
|
|
|
|
return true;
|
|
}
|
|
|
|
// circle_run - runs the circle flight mode
|
|
// should be called at 100hz or more
|
|
void ModeCircle::run()
|
|
{
|
|
// initialize speeds and accelerations
|
|
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
|
|
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
|
|
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
|
|
pos_control->set_max_accel_z(g.pilot_accel_z);
|
|
|
|
// get pilot's desired yaw rate (or zero if in radio failsafe)
|
|
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
|
if (!is_zero(target_yaw_rate)) {
|
|
pilot_yaw_override = true;
|
|
}
|
|
|
|
// get pilot desired climb rate (or zero if in radio failsafe)
|
|
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
|
|
// adjust climb rate using rangefinder
|
|
if (copter.rangefinder_alt_ok()) {
|
|
// if rangefinder is ok, use surface tracking
|
|
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
|
|
}
|
|
|
|
// if not armed set throttle to zero and exit immediately
|
|
if (is_disarmed_or_landed()) {
|
|
make_safe_spool_down();
|
|
return;
|
|
}
|
|
|
|
// set motors to full range
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
|
|
// run circle controller
|
|
copter.circle_nav->update();
|
|
|
|
// call attitude controller
|
|
if (pilot_yaw_override) {
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(),
|
|
copter.circle_nav->get_pitch(),
|
|
target_yaw_rate);
|
|
} else {
|
|
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(),
|
|
copter.circle_nav->get_pitch(),
|
|
copter.circle_nav->get_yaw(), true);
|
|
}
|
|
|
|
// update altitude target and call position controller
|
|
// protects heli's from inflight motor interlock disable
|
|
if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !copter.ap.land_complete) {
|
|
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
|
} else {
|
|
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
|
}
|
|
pos_control->update_z_controller();
|
|
}
|
|
|
|
uint32_t ModeCircle::wp_distance() const
|
|
{
|
|
return copter.circle_nav->get_distance_to_target();
|
|
}
|
|
|
|
int32_t ModeCircle::wp_bearing() const
|
|
{
|
|
return copter.circle_nav->get_bearing_to_target();
|
|
}
|
|
|
|
#endif
|