}
// 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);
get_stabilize_pitch(nav_pitch); break;
case ROLL_PITCH_SPORT:
// 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;
get_roll_rate_stabilized_ef(g.rc_1.control_in); get_pitch_rate_stabilized_ef(g.rc_2.control_in); break;
#if AUTOTUNE == ENABLED
case ROLL_PITCH_AUTOTUNE:
// apply SIMPLE mode transform
if(ap.simple_mode && ap.new_radio_frame) { 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);
// copy user input for reporting purposes
get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in); break; #endif } #if FRAME_CONFIG != HELI_FRAME
if(g.rc_3.control_in == 0 && control_mode <= ACRO) { reset_rate_I(); } #endif //HELI_FRAME
if(ap.new_radio_frame) {
// clear new radio frame info ap.new_radio_frame = false; } }
static void
init_simple_bearing() {
// capture current cos_yaw and sin_yaw values simple_cos_yaw = cos_yaw; simple_sin_yaw = sin_yaw;
// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000); super_simple_cos_yaw = simple_cos_yaw; super_simple_sin_yaw = simple_sin_yaw;
// log the simple bearing to dataflash if (g.log_bitmask != 0) {
Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor); }
}
// update_simple_mode - rotates pilot input if we are in simple mode void update_simple_mode(void) {
float rollx, pitchx;
// exit immediately if no new radio frame or not in simple mode if (ap.simple_mode == 0 || !ap.new_radio_frame) { return; }
if (ap.simple_mode == 1) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing) rollx = g.rc_1.control_in*simple_cos_yaw - g.rc_2.control_in*simple_sin_yaw; pitchx = g.rc_1.control_in*simple_sin_yaw + g.rc_2.control_in*simple_cos_yaw; }else{
// rotate roll, pitch input by -super simple heading (reverse of heading to home)
rollx = g.rc_1.control_in*super_simple_cos_yaw - g.rc_2.control_in*super_simple_sin_yaw; pitchx = g.rc_1.control_in*super_simple_sin_yaw + g.rc_2.control_in*super_simple_cos_yaw; }
// rotate roll, pitch input from north facing to vehicle's perspective g.rc_1.control_in = rollx*cos_yaw + pitchx*sin_yaw; g.rc_2.control_in = -rollx*sin_yaw + pitchx*cos_yaw; }
// update_super_simple_bearing - adjusts simple bearing based on location // should be called after home_bearing has been updated void update_super_simple_bearing(bool force_update) {
// check if we are in super simple mode and at least 10m from home
if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) { // check the bearing to home has changed by at least 5 degrees if (labs(super_simple_last_bearing - home_bearing) > 500) { super_simple_last_bearing = home_bearing;
float angle_rad = radians((super_simple_last_bearing+18000)/100); super_simple_cos_yaw = cosf(angle_rad); super_simple_sin_yaw = sinf(angle_rad); } } }
// set_throttle_mode - sets the throttle mode and initialises any variables as required bool set_throttle_mode( uint8_t new_throttle_mode ) {
// boolean to ensure proper initialisation of throttle modes bool throttle_initialised = false;
// return immediately if no change
if( new_throttle_mode == throttle_mode ) { return true; }
// initialise any variables required for the new throttle mode switch(new_throttle_mode) { case THROTTLE_MANUAL:
case THROTTLE_MANUAL_TILT_COMPENSATED:
throttle_accel_deactivate(); // this controller does not use accel based throttle controller
altitude_error = 0; // clear altitude error reported to GCS throttle_initialised = true;