// keep heading always pointing at home with no pilot input allowed nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); }
get_stabilize_yaw(nav_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }
break;
case YAW_LOOK_AT_HEADING:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }else{
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_heading, yaw_look_at_heading_slew); }
get_stabilize_yaw(nav_yaw); break; case YAW_LOOK_AHEAD:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; } // Commanded Yaw to automatically look ahead. get_look_ahead_yaw(g.rc_4.control_in); break;
#if TOY_LOOKUP == TOY_EXTERNAL_MIXER case YAW_TOY:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }else{
// update to allow external roll/yaw mixing
// keep heading always pointing at home with no pilot input allowed nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); }
get_stabilize_yaw(nav_yaw); break; #endif
case YAW_RESETTOARMEDYAW:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }else{
// changes yaw to be same as when quad was armed
nav_yaw = get_yaw_slew(nav_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE); }
get_stabilize_yaw(nav_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }
break; }
}
// get yaw mode based on WP_YAW_BEHAVIOR parameter // set rtl parameter to true if this is during an RTL uint8_t get_wp_yaw_mode(bool rtl) {
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: return YAW_LOOK_AT_NEXT_WP; break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: if( rtl ) {
return YAW_HOLD; }else{
return YAW_LOOK_AT_NEXT_WP; }
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD: return YAW_LOOK_AHEAD; break;
default:
return YAW_HOLD; break; } }
// set_roll_pitch_mode - update roll/pitch mode and initialise any variables as required bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) {
// boolean to ensure proper initialisation of throttle modes bool roll_pitch_initialised = false;
// return immediately if no change
if( new_roll_pitch_mode == roll_pitch_mode ) { return true; }
switch( new_roll_pitch_mode ) { case ROLL_PITCH_STABLE:
roll_pitch_initialised = true; break;
case ROLL_PITCH_ACRO:
// reset acro level rates acro_roll_rate = 0; acro_pitch_rate = 0;
roll_pitch_initialised = true; break;
case ROLL_PITCH_AUTO:
case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_TOY: case ROLL_PITCH_SPORT:
roll_pitch_initialised = true; break;
case ROLL_PITCH_LOITER: // require gps lock if( ap.home_is_set ) {
roll_pitch_initialised = true; }
break;
#if AUTOTUNE == ENABLED
case ROLL_PITCH_AUTOTUNE:
// only enter autotune mode from stabilized roll-pitch mode when armed and flying if (roll_pitch_mode == ROLL_PITCH_STABLE && motors.armed() && !ap.land_complete) {
// auto_tune_start returns true if it wants the roll-pitch mode changed to autotune roll_pitch_initialised = auto_tune_start(); }
break; #endif }
// if initialisation has been successful update the yaw mode if( roll_pitch_initialised ) {
exit_roll_pitch_mode(roll_pitch_mode); roll_pitch_mode = new_roll_pitch_mode; }
// return success or failure return roll_pitch_initialised; }
// exit_roll_pitch_mode - peforms any code required when exiting the current roll-pitch mode void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode) {
#if AUTOTUNE == ENABLED
if (old_roll_pitch_mode == ROLL_PITCH_AUTOTUNE) { auto_tune_stop(); } #endif }
// update_roll_pitch_mode - run high level roll and pitch controllers // 100hz update rate
void update_roll_pitch_mode(void) {
switch(roll_pitch_mode) { case ROLL_PITCH_ACRO:
// copy user input for reporting purposes control_roll = g.rc_1.control_in; control_pitch = g.rc_2.control_in;
#if FRAME_CONFIG == HELI_FRAME
// ACRO does not get SIMPLE mode ability if (motors.flybar_mode == 1) {
g.rc_1.servo_out = g.rc_1.control_in; g.rc_2.servo_out = g.rc_2.control_in; }else{
acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
get_roll_rate_stabilized_bf(g.rc_1.control_in); get_pitch_rate_stabilized_bf(g.rc_2.control_in); get_acro_level_rates(); }
#else // !HELI_FRAME
acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
get_roll_rate_stabilized_bf(g.rc_1.control_in); get_pitch_rate_stabilized_bf(g.rc_2.control_in); get_acro_level_rates();
#endif // HELI_FRAME break;
case ROLL_PITCH_STABLE:
// apply SIMPLE mode transform update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// pass desired roll, pitch to stabilize attitude controllers get_stabilize_roll(control_roll); get_stabilize_pitch(control_pitch);
break;
case ROLL_PITCH_AUTO:
// copy latest output from nav controller to stabilize controller nav_roll = wp_nav.get_desired_roll(); nav_pitch = wp_nav.get_desired_pitch(); get_stabilize_roll(nav_roll); get_stabilize_pitch(nav_pitch);
// user input, although ignored is put into control_roll and pitch for reporting purposes control_roll = g.rc_1.control_in; control_pitch = g.rc_2.control_in; break;
case ROLL_PITCH_STABLE_OF:
// apply SIMPLE mode transform update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// mix in user control with optical flow
get_stabilize_roll(get_of_roll(control_roll)); get_stabilize_pitch(get_of_pitch(control_pitch)); break;
// THOR
// a call out to the main toy logic case ROLL_PITCH_TOY: roll_pitch_toy(); break;
case ROLL_PITCH_LOITER:
// apply SIMPLE mode transform update_simple_mode();
// copy user input for reporting purposes control_roll = g.rc_1.control_in; control_pitch = g.rc_2.control_in;
// update loiter target from user controls
wp_nav.move_loiter_target(control_roll, control_pitch,0.01f);
// copy latest output from nav controller to stabilize controller nav_roll = wp_nav.get_desired_roll(); nav_pitch = wp_nav.get_desired_pitch();
get_stabilize_roll(nav_roll);