void ModeSmartRTL::update()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
smart_rtl_state = SmartRTL_PathFollow;
_load_point = true;
}
// Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely)
stop_vehicle();
break;
case SmartRTL_PathFollow:
// load point if required
if (_load_point) {
Vector3f next_point;
if (!g2.smart_rtl.pop_point(next_point)) {
// if not more points, we have reached home
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
smart_rtl_state = SmartRTL_StopAtHome;
break;
}
_load_point = false;
// set target destination to new point
if (!set_desired_location_NED(next_point)) {
// this failure should never happen but we add it just in case
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
}
}
// check if we've reached the next point
_distance_to_destination = get_distance(rover.current_loc, _destination);
if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
_load_point = true;
}
// continue driving towards destination
calc_steering_to_waypoint(_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
break;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
_reached_destination = true;
if (rover.is_boat()) {
// boats attempt to hold position at home
calc_steering_to_waypoint(rover.current_loc, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
// rovers stop
stop_vehicle();
}
break;
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this should never be reached
set_mode(RTL);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
if (auto_state.land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
calc_throttle();
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->set_servo_out(0);
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
auto_state.land_complete = false;
auto_state.land_pre_flare = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint8_t nav_cmd_id;
// we should be either running a mission or RTLing home
if (mission.state() == AP_Mission::MISSION_RUNNING) {
nav_cmd_id = mission.get_current_nav_cmd().id;
}else{
nav_cmd_id = auto_rtl_command.id;
}
switch(nav_cmd_id) {
case MAV_CMD_NAV_TAKEOFF:
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
break;
case MAV_CMD_NAV_LAND:
calc_nav_roll();
calc_nav_pitch();
if (auto_state.land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
calc_throttle();
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
}
break;
default:
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
steer_state.hold_course_cd = -1;
auto_state.land_complete = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
}
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
_desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(_desired_speed, false, true);
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
void Plane::update_fbwb_speed_height(void)
{
static float last_elevator_input;
float elevator_input;
elevator_input = channel_pitch->control_in / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f);
if (is_zero(elevator_input) && !is_zero(last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
set_target_altitude_current();
}
// check for FBWB altitude limit
check_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
last_elevator_input = elevator_input;
calc_throttle();
calc_nav_pitch();
}
// output_armed - sends commands to the motors
void AP_MotorsVtol::output_armed()
{
int16_t motor_out[VTOL_NUMBERS_OF_MOTORS + VTOL_NUMBERS_OF_SERVOS];
float transition_factor = get_transition_progress();
for (uint8_t i = 0; i < VTOL_NUMBERS_OF_MOTORS + VTOL_NUMBERS_OF_SERVOS; i++)
{
motor_out[i] = 0;
}
set_throttle_limits();
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll.calc_pwm();
_rc_pitch.calc_pwm();
_rc_yaw.calc_pwm();
_rc_throttle.calc_pwm();
calc_throttle(motor_out);
calc_vertical_roll_pitch(motor_out, transition_factor);
calc_horizontal_roll_pitch(motor_out, transition_factor);
calc_yaw_and_transition(motor_out, transition_factor);
output_motors(motor_out);
}
void Rover::do_yaw(const AP_Mission::Mission_Command& cmd)
{
// Only support target yaw for now
condition_start = condition_value; // save condition_value from current navigation wp loaded
// get final angle, 1 = Relative, 0 = Absolute
if (cmd.content.yaw.relative_angle == 0) {
// absolute angle
condition_value = cmd.content.yaw.angle_deg * 100;
} else {
// relative angle
condition_value = cmd.content.yaw.angle_deg * 100;
if (cmd.content.yaw.direction < 0) {
condition_value = -condition_value;
}
condition_value = condition_value + ahrs.yaw_sensor;
}
// absolute angle error
const int32_t error_to_target_yaw = abs((condition_value - ahrs.yaw_sensor));
// Calculate the steering to apply base on error calculated
const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
next_navigation_leg_cd = condition_value;
calc_throttle(g.speed_cruise);
do_auto_rotation = true;
}
void Rover::nav_set_speed()
{
// if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
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.0f;
prev_WP = current_loc;
next_WP = current_loc;
set_guided_WP(current_loc); // exit Guided_Velocity to prevent spam
return;
}
prev_WP = current_loc;
next_WP = current_loc;
const int32_t steer_value = steerController.get_steering_out_rate(guided_control.target_steer_speed);
location_update(next_WP, (steer_value + ahrs.yaw_sensor) * 0.01f, 4.0f); // put the next wp at 4m forward at steer direction
nav_controller->update_waypoint(current_loc, next_WP);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
calc_throttle(guided_control.target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
set_mode(RTL, MODE_REASON_MISSION_END);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
if (landing.is_complete()) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->set_servo_out(0);
} else {
calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
// we should be either running a mission or RTLing home
if (mission.state() == AP_Mission::MISSION_RUNNING) {
nav_cmd_id = mission.get_current_nav_cmd().id;
}else{
nav_cmd_id = auto_rtl_command.id;
}
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
if (auto_state.land_complete) {
// during final approach constrain roll to the range
// allowed for level flight
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
}
calc_throttle();
if (auto_state.land_complete) {
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->servo_out = 0;
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
auto_state.land_complete = false;
auto_state.land_pre_flare = false;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main handling for AUTO mode
*/
void Plane::handle_auto_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
set_mode(RTL, MODE_REASON_MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
// allow landing to restrict the roll limits
nav_roll_cd = landing.constrain_roll(nav_roll_cd, g.level_roll_limit*100UL);
if (landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
void ModeSteering::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
_desired_lat_accel = 0.0f;
return;
}
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// In steering mode we control lateral acceleration directly.
// For pivot steering vehicles we use the TURN_MAX_G parameter
// For regular steering vehicles we use the maximum lateral acceleration at full steering lock for this speed: V^2/R where R is the radius of turn.
float max_g_force;
if (is_pivot_turning) {
max_g_force = g.turn_max_g * GRAVITY_MSS;
} else {
max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
}
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
_desired_lat_accel = max_g_force * (desired_steering / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
_desired_lat_accel = -_desired_lat_accel;
}
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle();
} else {
// run lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(false);
calc_throttle(target_speed, false);
}
}
void ModeAcro::update()
{
// get speed forward
float speed, desired_steering;
if (!attitude_control.get_forward_speed(speed)) {
float desired_throttle;
// convert pilot stick input into desired steering and throttle
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// no valid speed, just use the provided throttle
g2.motors.set_throttle(desired_throttle);
} else {
float desired_speed;
// convert pilot stick input into desired steering and speed
get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
calc_throttle(desired_speed, true);
}
float steering_out;
// handle sailboats
if (!is_zero(desired_steering)) {
// steering input return control to user
rover.sailboat_clear_tack();
}
if (g2.motors.has_sail() && rover.sailboat_tacking()) {
// call heading controller during tacking
steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
g2.wp_nav.get_pivot_rate(),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
} else {
// convert pilot steering input to desired turn rate in radians/sec
const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
// run steering turn rate controller and throttle controller
steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
}
set_steering(steering_out * 4500.0f);
}
void ModeSteering::update()
{
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = channel_throttle->get_control_in() * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
return;
}
// 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.
float max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
lateral_acceleration = -lateral_acceleration;
}
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed)) {
stop_vehicle();
} else {
// run steering controller
calc_nav_steer(reversed);
calc_throttle(target_speed, false);
}
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
if (is_negative(_desired_speed)) {
_desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
} else {
_desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
}
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// note: copied from calc_reduced_speed_for_turn_or_distance
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, false, true);
// mark us as in_reverse when using a negative throttle
// To-Do: only in reverse if vehicle is actually travelling backwards?
rover.set_reverse(_desired_speed < 0);
}
void ModeRTL::update()
{
if (!_reached_destination || rover.is_boat()) {
// calculate distance to home
_distance_to_destination = get_distance(rover.current_loc, _destination);
// check if we've reached the destination
if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) {
// trigger reached
_reached_destination = true;
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
}
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
// we've reached destination so stop
stop_vehicle();
_desired_lat_accel = 0.0f;
}
}
void ModeRTL::update()
{
// calculate distance to home
_distance_to_destination = rover.current_loc.get_distance(_destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
// trigger reached
_reached_destination = true;
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
}
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
} else {
// we've reached destination so stop
stop_vehicle();
}
}
void Rover::nav_set_yaw_speed()
{
// if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
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.0f;
return;
}
const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
// speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed
// 1 is double the cruise speed.
const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f;
calc_throttle(target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f));
}
bool Rover::do_yaw_rotation()
{
// absolute angle error
const int32_t error_to_target_yaw = abs(condition_value - ahrs.yaw_sensor);
// check if we are within 5 degrees of the target heading
if (error_to_target_yaw <= 500) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); // stop the current rotation
condition_value = condition_start; // reset the condition value to its previous value
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
do_auto_rotation = false;
return true;
} else {
// Calculate the steering to apply base on error calculated
const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
calc_throttle(g.speed_cruise);
do_auto_rotation = true;
return false;
}
}
void Rover::nav_set_yaw_speed()
{
// if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_yaw_speed.msg_time_ms) > 3000)
{
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
return;
}
channel_steer->set_servo_out(steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle));
// speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed
// 1 is double the cruise speed.
float target_speed = g.speed_cruise * guided_yaw_speed.target_speed * 2;
calc_throttle(target_speed);
return;
}
void ModeSimple::update()
{
float desired_heading_cd, desired_speed;
// get pilot input
get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);
// rotate heading around based on initial heading
if (g2.simple_type == Simple_InitialHeading) {
desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd);
}
// if sticks in middle, use previous desired heading (important when vehicle is slowing down)
if (!is_positive(desired_speed)) {
desired_heading_cd = _desired_heading_cd;
} else {
// record desired heading for next iteration
_desired_heading_cd = desired_heading_cd;
}
// run throttle and steering controllers
calc_steering_to_heading(desired_heading_cd);
calc_throttle(desired_speed, true);
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
void Plane::update_fbwb_speed_height(void)
{
uint32_t now = micros();
if (now - target_altitude.last_elev_check_us >= 100000) {
// we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and
// give below 1cm altitude change, which would result in no climb or descent
float dt = (now - target_altitude.last_elev_check_us) * 1.0e-6;
dt = constrain_float(dt, 0.1, 0.15);
target_altitude.last_elev_check_us = now;
float elevator_input = channel_pitch->get_control_in() / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100;
change_target_altitude(alt_change_cm);
if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
set_target_altitude_current();
}
target_altitude.last_elevator_input = elevator_input;
}
check_fbwb_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
calc_throttle();
calc_nav_pitch();
}
/*
main flight mode dependent update code
*/
void Plane::update_flight_mode(void)
{
enum FlightMode effective_mode = control_mode;
if (control_mode == AUTO && g.auto_fbw_steer == 42) {
effective_mode = FLY_BY_WIRE_A;
}
if (effective_mode != AUTO) {
// hold_course is only used in takeoff and landing
steer_state.hold_course_cd = -1;
}
// ensure we are fly-forward when we are flying as a pure fixed
// wing aircraft. This helps the EKF produce better state
// estimates as it can make stronger assumptions
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
} else {
ahrs.set_fly_forward(true);
}
switch (effective_mode)
{
case AUTO:
handle_auto_mode();
break;
case AVOID_ADSB:
case GUIDED:
if (auto_state.vtol_loiter && quadplane.available()) {
quadplane.guided_update();
break;
}
FALLTHROUGH;
case RTL:
case LOITER:
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case TRAINING: {
training_manual_roll = false;
training_manual_pitch = false;
update_load_factor();
// if the roll is past the set roll limit, then
// we set target roll to the limit
if (ahrs.roll_sensor >= roll_limit_cd) {
nav_roll_cd = roll_limit_cd;
} else if (ahrs.roll_sensor <= -roll_limit_cd) {
nav_roll_cd = -roll_limit_cd;
} else {
training_manual_roll = true;
nav_roll_cd = 0;
}
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
nav_pitch_cd = pitch_limit_min_cd;
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
}
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
}
case ACRO: {
// handle locked/unlocked control
if (acro_state.locked_roll) {
nav_roll_cd = acro_state.locked_roll_err;
} else {
nav_roll_cd = ahrs.roll_sensor;
}
if (acro_state.locked_pitch) {
nav_pitch_cd = acro_state.locked_pitch_cd;
} else {
nav_pitch_cd = ahrs.pitch_sensor;
}
break;
}
case AUTOTUNE:
case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
update_load_factor();
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
}
adjust_nav_pitch_throttle();
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) {
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
if (g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
bool tdrag_mode = (RC_Channels::get_radio_in(g.fbwa_tdrag_chan-1) > 1700);
if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
auto_state.fbwa_tdrag_takeoff_mode = true;
gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
}
}
}
break;
}
case FLY_BY_WIRE_B:
// Thanks to Yury MonZon for the altitude limit code!
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
update_load_factor();
update_fbwb_speed_height();
break;
case CRUISE:
/*
in CRUISE mode we use the navigation code to control
roll when heading is locked. Heading becomes unlocked on
any aileron or rudder input
*/
if (channel_roll->get_control_in() != 0 || channel_rudder->get_control_in() != 0) {
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
}
if (!cruise_state.locked_heading) {
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
update_load_factor();
} else {
calc_nav_roll();
}
update_fbwb_speed_height();
break;
case STABILIZE:
nav_roll_cd = 0;
nav_pitch_cd = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS,
// holding altitude at the altitude we set when we
// switched into the mode
nav_roll_cd = roll_limit_cd / 3;
update_load_factor();
calc_nav_pitch();
calc_throttle();
break;
case MANUAL:
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
steering_control.steering = steering_control.rudder = channel_rudder->get_control_in_zero_dz();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL: {
// set nav_roll and nav_pitch using sticks
int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max);
float pitch_input = channel_pitch->norm_input();
// Scale from normalized input [-1,1] to centidegrees
if (quadplane.tailsitter_active()) {
// separate limit for tailsitter roll, if set
if (quadplane.tailsitter.max_roll_angle > 0) {
roll_limit = quadplane.tailsitter.max_roll_angle * 100.0f;
}
// angle max for tailsitter pitch
nav_pitch_cd = pitch_input * quadplane.aparm.angle_max;
} else {
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
// tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max);
} else {
nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}
nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
break;
}
case INITIALISING:
// handled elsewhere
break;
}
}
static void *control_loop(void *arg)
{
// Set thread name
prctl(1, "fixedwing_control attitude", getpid());
control_outputs.mode = HIL_MODE + 16;
control_outputs.nav_mode = 0;
while (1) {
/************************************
* Read global data structures here
************************************/
// Get parameters
get_parameters();
#define MUTE
#ifndef MUTE
///// DEBUG OUTPUT ////////
// printf("Throttle: \tChannel %i\t Value %i\n", global_data_rc_channels->function[THROTTLE], global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale);
// printf("Throttle_correct: \tChannel %i\t Value %i\n", THROTTLE, global_data_rc_channels->chan[THROTTLE].scale);
// printf("Override: Channel %i\t Value %i\n", global_data_rc_channels->function[OVERRIDE], global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale);
// printf("Override_correct: \tChannel %i\t Value %i\n", OVERRIDE, global_data_rc_channels->chan[OVERRIDE].scale);
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
printf("Current Altitude: %i\n\n", (int)plane_data.alt);
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
(int)(1000 * plane_data.rollspeed), (int)(1000 * plane_data.pitchspeed), (int)(1000 * plane_data.yawspeed));
printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
(int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
// printf("\nGlobal attitude outputs \n, %i\n%i\n%i\n", global_data_fixedwing_control->attitude_control_output[ROLL],
// global_data_fixedwing_control->attitude_control_output[PITCH], global_data_fixedwing_control->attitude_control_output[THROTTLE]);
///////////////////////////
#endif
// position values
if (global_data_trylock(&global_pos.access_conf) == 0) {
plane_data.lat = global_pos.lat / 10000000;
plane_data.lon = global_pos.lon / 10000000;
plane_data.alt = global_pos.alt / 1000;
plane_data.vx = global_pos.vx / 100;
plane_data.vy = global_pos.vy / 100;
plane_data.vz = global_pos.vz / 100;
}
global_data_unlock(&global_pos.access_conf);
// attitude values
if (global_data_trylock(&att.access_conf) == 0) {
plane_data.roll = att.roll;
plane_data.pitch = att.pitch;
plane_data.yaw = att.yaw;
plane_data.hdg = att.yaw - 180;
plane_data.rollspeed = att.rollspeed;
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
}
global_data_unlock(&att.access_conf);
// Set plane mode
set_plane_mode();
// Calculate the P,Q,R body rates of the aircraft
calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG,
plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
// Calculate the body frame angles of the aircraft
//calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG);
// Calculate the output values
control_outputs.roll_ailerons = calc_roll_ail();
control_outputs.pitch_elevator = calc_pitch_elev();
//control_outputs.yaw_rudder = calc_yaw_rudder();
control_outputs.throttle = calc_throttle();
/*******************************************
* Send data to global data structure
******************************************/
if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
if (plane_data.mode == TAKEOFF) {
global_data_fixedwing_control->attitude_control_output[ROLL] = 0;
global_data_fixedwing_control->attitude_control_output[PITCH] = 5000;
global_data_fixedwing_control->attitude_control_output[THROTTLE] = 10000;
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
if (plane_data.mode == CRUISE) {
global_data_fixedwing_control->attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons);
global_data_fixedwing_control->attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator);
global_data_fixedwing_control->attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle);
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
global_data_fixedwing_control->counter++;
global_data_fixedwing_control->timestamp = hrt_absolute_time();
#error Either publish here or above, not at two locations
orb_publish()
}
// 20 Hz loop
usleep(100000);
}
return NULL;
}
int fixedwing_control_main(int argc, char *argv[])
{
/* default values for arguments */
char *fixedwing_uart_name = "/dev/ttyS1";
char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
fixedwing_uart_name = argv[i + 1];
} else {
printf(commandline_usage, argv[0]);
return 0;
}
}
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing control] started\n");
/* Set up to publish fixed wing control messages */
struct fixedwing_control_s control;
int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
/* Subscribe to global position, attitude and rc */
struct vehicle_global_position_s global_pos;
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_attitude_s att;
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct rc_channels_s rc;
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
struct vehicle_global_position_setpoint_s global_setpoint;
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* Mainloop setup */
unsigned int loopcounter = 0;
unsigned int failcounter = 0;
/* Control constants */
control_outputs.mode = HIL_MODE;
control_outputs.nav_mode = 0;
/* Servo setup */
int fd;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
fd = open("/dev/pwm_servo", O_RDWR);
if (fd < 0) {
printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
}
ioctl(fd, PWM_SERVO_ARM, 0);
int16_t buffer_rc[3];
int16_t buffer_servo[3];
mixer_data_t mixer_buffer;
mixer_buffer.input = buffer_rc;
mixer_buffer.output = buffer_servo;
mixer_conf_t mixers[3];
mixers[0].source = PITCH;
mixers[0].nr_actuators = 2;
mixers[0].dest[0] = AIL_1;
mixers[0].dest[1] = AIL_2;
mixers[0].dual_rate[0] = 1;
mixers[0].dual_rate[1] = 1;
mixers[1].source = ROLL;
mixers[1].nr_actuators = 2;
mixers[1].dest[0] = AIL_1;
mixers[1].dest[1] = AIL_2;
mixers[1].dual_rate[0] = 1;
mixers[1].dual_rate[1] = -1;
mixers[2].source = THROTTLE;
mixers[2].nr_actuators = 1;
mixers[2].dest[0] = MOT;
mixers[2].dual_rate[0] = 1;
/*
* Main control, navigation and servo routine
*/
while(1) {
/*
* DATA Handling
* Fetch current flight data
*/
/* get position, attitude and rc inputs */
// XXX add error checking
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* scaling factors are defined by the data from the APM Planner
* TODO: ifdef for other parameters (HIL/Real world switch)
*/
/* position values*/
plane_data.lat = global_pos.lat; /// 10000000.0;
plane_data.lon = global_pos.lon; /// 10000000.0;
plane_data.alt = global_pos.alt; /// 1000.0f;
plane_data.vx = global_pos.vx / 100.0f;
plane_data.vy = global_pos.vy / 100.0f;
plane_data.vz = global_pos.vz / 100.0f;
/* attitude values*/
plane_data.roll = att.roll;
plane_data.pitch = att.pitch;
plane_data.yaw = att.yaw;
plane_data.rollspeed = att.rollspeed;
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
/* parameter values */
get_parameters(&plane_data, &pid_s);
/* Attitude control part */
if (verbose && loopcounter % 20 == 0) {
/******************************** DEBUG OUTPUT ************************************************************/
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)pid_s.Kp_att, (int)pid_s.Ki_att,
(int)pid_s.Kd_att, (int)pid_s.intmax_att, (int)pid_s.Kp_pos, (int)pid_s.Ki_pos,
(int)pid_s.Kd_pos, (int)pid_s.intmax_pos, (int)plane_data.airspeed,
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z,
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPALT],
global_setpoint.lat,
global_setpoint.lon,
(int)global_setpoint.altitude);
printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*plane_data.throttle_setpoint));
printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed(&plane_data)));
printf("Current Position: \n %i \n %i \n %i \n", (int)plane_data.lat, (int)plane_data.lon, (int)plane_data.alt);
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
(int)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
(int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
(int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator),
(int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle));
/************************************************************************************************************/
}
/*
* Computation section
*
* The function calls to compute the required control values happen
* in this section.
*/
/* Set plane mode */
set_plane_mode(&plane_data);
/* Calculate the output values */
control_outputs.roll_ailerons = calc_roll_ail(&plane_data, &pid_s);
control_outputs.pitch_elevator = calc_pitch_elev(&plane_data, &pid_s);
control_outputs.yaw_rudder = calc_yaw_rudder(&plane_data, &pid_s);
control_outputs.throttle = calc_throttle(&plane_data, &pid_s);
/*
* Calculate rotation matrices
*/
calc_rotation_matrix(&rmatrix_att, plane_data.roll, plane_data.pitch, plane_data.yaw);
calc_rotation_matrix(&rmatrix_c, control_outputs.roll_ailerons, control_outputs.pitch_elevator, control_outputs.yaw_rudder);
multiply_matrices(&rmatrix_att, &rmatrix_c, &rmatrix_b);
calc_angles_from_rotation_matrix(&rmatrix_b, plane_data.rollb, plane_data.pitchb, plane_data.yawb);
// TODO: fix RC input
//if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
/*
* TAKEOFF hack for HIL
*/
if (plane_data.mode == TAKEOFF) {
control.attitude_control_output[ROLL] = 0;
control.attitude_control_output[PITCH] = 5000;
control.attitude_control_output[THROTTLE] = 10000;
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
if (plane_data.mode == CRUISE) {
// TODO: substitute output with the body angles (rollb, pitchb)
control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
control.attitude_control_output[THROTTLE] = control_outputs.throttle;
//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
control.counter++;
control.timestamp = hrt_absolute_time();
//}
/* Navigation part */
/*
* TODO: APM Planner Waypoint communication
*/
// Get GPS Waypoint
// plane_data.wp_x = global_setpoint.lon;
// plane_data.wp_y = global_setpoint.lat;
// plane_data.wp_z = global_setpoint.altitude;
/*
* TODO: fix RC input
*/
//if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode
// AUTO/HYBRID switch
//if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) {
plane_data.roll_setpoint = calc_roll_setpoint(35.0f, &plane_data);
plane_data.pitch_setpoint = calc_pitch_setpoint(35.0f, &plane_data);
plane_data.throttle_setpoint = calc_throttle_setpoint(&plane_data);
// } else {
// plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200;
// plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
// plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
// }
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
// } else {
// control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000;
// control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000;
// control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000;
// since we don't have a yaw rudder
//control.attitude_control_output[YAW] = rc.chan[rc.function[YAW]].scale/10000;
control.counter++;
control.timestamp = hrt_absolute_time();
//}
/* publish the control data */
orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
/* Servo part */
buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);
//mix_and_link(mixers, 3, 2, &mixer_buffer);
// Scaling and saturation of servo outputs happens here
data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];
if (data[AIL_1] > SERVO_MAX)
data[AIL_1] = SERVO_MAX;
if (data[AIL_1] < SERVO_MIN)
data[AIL_1] = SERVO_MIN;
data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];
if (data[AIL_2] > SERVO_MAX)
data[AIL_2] = SERVO_MAX;
if (data[AIL_2] < SERVO_MIN)
data[AIL_2] = SERVO_MIN;
data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];
if (data[MOT] > SERVO_MAX)
data[MOT] = SERVO_MAX;
if (data[MOT] < SERVO_MIN)
data[MOT] = SERVO_MIN;
int result = write(fd, &data, sizeof(data));
if (result != sizeof(data)) {
if (failcounter < 10 || failcounter % 20 == 0) {
printf("[fixedwing_control] failed writing servo outputs\n");
}
failcounter++;
}
loopcounter++;
/* 20Hz loop*/
usleep(50000);
}
return 0;
}
void ModeGuided::update()
{
switch (_guided_mode) {
case Guided_WP:
{
if (!_reached_destination || rover.is_boat()) {
// check if we've reached the destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) {
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
stop_vehicle();
}
break;
}
case Guided_HeadingAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true);
} else {
stop_vehicle();
g2.motors.set_steering(0.0f);
}
break;
}
case Guided_TurnRateAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true);
} else {
stop_vehicle();
g2.motors.set_steering(0.0f);
}
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
}
/*
main flight mode dependent update code
*/
void Plane::update_flight_mode(void)
{
enum FlightMode effective_mode = control_mode;
if (control_mode == AUTO && g.auto_fbw_steer == 42) {
effective_mode = FLY_BY_WIRE_A;
}
if (effective_mode != AUTO) {
// hold_course is only used in takeoff and landing
steer_state.hold_course_cd = -1;
}
// ensure we are fly-forward
if (quadplane.in_vtol_mode()) {
ahrs.set_fly_forward(false);
} else {
ahrs.set_fly_forward(true);
}
switch (effective_mode)
{
case AUTO:
handle_auto_mode();
break;
case GUIDED:
if (auto_state.vtol_loiter && quadplane.available()) {
quadplane.guided_update();
break;
}
// fall through
case RTL:
case LOITER:
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case TRAINING: {
training_manual_roll = false;
training_manual_pitch = false;
update_load_factor();
// if the roll is past the set roll limit, then
// we set target roll to the limit
if (ahrs.roll_sensor >= roll_limit_cd) {
nav_roll_cd = roll_limit_cd;
} else if (ahrs.roll_sensor <= -roll_limit_cd) {
nav_roll_cd = -roll_limit_cd;
} else {
training_manual_roll = true;
nav_roll_cd = 0;
}
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
nav_pitch_cd = pitch_limit_min_cd;
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
}
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
}
case ACRO: {
// handle locked/unlocked control
if (acro_state.locked_roll) {
nav_roll_cd = acro_state.locked_roll_err;
} else {
nav_roll_cd = ahrs.roll_sensor;
}
if (acro_state.locked_pitch) {
nav_pitch_cd = acro_state.locked_pitch_cd;
} else {
nav_pitch_cd = ahrs.pitch_sensor;
}
break;
}
case AUTOTUNE:
case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
update_load_factor();
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
}
adjust_nav_pitch_throttle();
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
channel_throttle->set_servo_out(0);
}
if (g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700);
if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
auto_state.fbwa_tdrag_takeoff_mode = true;
gcs_send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
}
}
}
break;
}
case FLY_BY_WIRE_B:
// Thanks to Yury MonZon for the altitude limit code!
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
update_load_factor();
update_fbwb_speed_height();
break;
case CRUISE:
/*
in CRUISE mode we use the navigation code to control
roll when heading is locked. Heading becomes unlocked on
any aileron or rudder input
*/
if ((channel_roll->get_control_in() != 0 ||
rudder_input != 0)) {
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
}
if (!cruise_state.locked_heading) {
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
update_load_factor();
} else {
calc_nav_roll();
}
update_fbwb_speed_height();
break;
case STABILIZE:
nav_roll_cd = 0;
nav_pitch_cd = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS,
// holding altitude at the altitude we set when we
// switched into the mode
nav_roll_cd = roll_limit_cd / 3;
update_load_factor();
calc_nav_pitch();
calc_throttle();
break;
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
channel_roll->set_servo_out(channel_roll->pwm_to_angle());
channel_pitch->set_servo_out(channel_pitch->pwm_to_angle());
steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL: {
// set nav_roll and nav_pitch using sticks
int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max);
nav_roll_cd = channel_roll->norm_input() * roll_limit;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max);
} else {
nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
break;
}
case INITIALISING:
// handled elsewhere
break;
}
}
void ModeGuided::update()
{
switch (_guided_mode) {
case Guided_WP:
{
_distance_to_destination = get_distance(rover.current_loc, _destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
} else {
stop_vehicle();
}
break;
}
case Guided_HeadingAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
} else {
stop_vehicle();
}
break;
}
case Guided_TurnRateAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true, true);
} else {
stop_vehicle();
}
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
}
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;
}
}
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;
}
}
