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;
break;
case THROTTLE_HOLD: case THROTTLE_AUTO:
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
wp_nav.set_desired_alt(controller_desired_alt); // same as above but for loiter controller
if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual reset_throttle_I();
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); }
throttle_initialised = true; break;
case THROTTLE_LAND:
reset_land_detector(); // initialise land detector
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude throttle_initialised = true; break; }
// update the throttle mode if( throttle_initialised ) {
throttle_mode = new_throttle_mode;
// reset some variables used for logging desired_climb_rate = 0; nav_throttle = 0; }
// return success or failure return throttle_initialised; }
// update_throttle_mode - run high level throttle controllers // 50 hz update rate
void update_throttle_mode(void) {
int16_t pilot_climb_rate;
int16_t pilot_throttle_scaled;
if(ap.do_flip) // this is pretty bad but needed to flip in AP modes. return;
#if FRAME_CONFIG == HELI_FRAME if (control_mode == STABILIZE){ motors.stab_throttle = true; } else { motors.stab_throttle = false; }
// allow swash collective to move if we are in manual throttle modes, even if disarmed if( !motors.armed() ) {
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){ throttle_accel_deactivate(); // do not allow the accel based throttle to override our command return; } }
#else // HELI_FRAME
// do not run throttle controllers if motors disarmed if( !motors.armed() ) {
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command set_target_alt_for_reporting(0); return; }
#endif // HELI_FRAME
switch(throttle_mode) {
case THROTTLE_MANUAL:
// completely manual throttle if(g.rc_3.control_in <= 0){ set_throttle_out(0, false); }else{
// send pilot's output directly to motors
pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); set_throttle_out(pilot_throttle_scaled, false);
// update estimate of throttle cruise #if FRAME_CONFIG == HELI_FRAME
update_throttle_cruise(motors.coll_out); #else update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME
// check if we've taken off yet
if (!ap.takeoff_complete && motors.armed()) {
if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true); } } }
set_target_alt_for_reporting(0); break;
case THROTTLE_MANUAL_TILT_COMPENSATED:
// manual throttle but with angle boost if (g.rc_3.control_in <= 0) {
set_throttle_out(0, false); // no need for angle boost with zero throttle }else{
pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); set_throttle_out(pilot_throttle_scaled, true);
// update estimate of throttle cruise #if FRAME_CONFIG == HELI_FRAME
update_throttle_cruise(motors.coll_out); #else update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME
if (!ap.takeoff_complete && motors.armed()) {
if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true);