ACRO Mode

A

//////////////////////////////////////////////////////////////////////////////// //ArduCopter.pde //////////////////////////////////////////////////////////////////////////////// void setup() { cliSerial = hal.console; // Load the default values of variables listed in var_info[]s AP_Param::setup_sketch_defaults(); init_ardupilot(); // initialise the main loop scheduler scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0])); } void loop() { // wait for an INS sample if (!ins.wait_for_sample(1000)) { Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY); return; } uint32_t timer = micros(); // check loop time perf_info_check_loop_time(timer - fast_loopTimer); // used by PI Loops G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; fast_loopTimer = timer; // for mainloop failure monitoring mainLoop_count++; // Execute the fast loop // --------------------- fast_loop(); B // Main loop - 100hz static void fast_loop() { // IMU DCM Algorithm // -------------------- read_AHRS(); C static void read_AHRS(void) { // Perform IMU calculations and get attitude info //----------------------------------------------- #if HIL_MODE != HIL_MODE_DISABLED // update hil before ahrs update gcs_check_input(); #endif ahrs.update(); }     // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); // write out the servo PWM values // ------------------------------ set_servos_4(); // Inertial Nav // -------------------- read_inertia(); // run the attitude controllers update_flight_mode(); D // update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more static void update_flight_mode() { switch (control_mode) { case ACRO: #if FRAME_CONFIG == HELI_FRAME heli_acro_run(); #else acro_run(); #endif break; case STABILIZE: #if FRAME_CONFIG == HELI_FRAME heli_stabilize_run(); #else stabilize_run(); #endif break; case ALT_HOLD: althold_run(); break; case AUTO: auto_run(); break; case CIRCLE: circle_run(); break; case LOITER: loiter_run(); break; case GUIDED: guided_run(); break; case LAND: land_run(); break; case RTL: rtl_run(); break; case OF_LOITER: ofloiter_run(); break; case DRIFT: drift_run(); break; case SPORT: sport_run(); break; case FLIP: flip_run(); break; #if AUTOTUNE_ENABLED == ENABLED case AUTOTUNE: autotune_run(); break; #endif #if HYBRID_ENABLED == ENABLED case HYBRID: hybrid_run(); break; #endif } } // optical flow // -------------------- #if OPTFLOW == ENABLED if(g.optflow_enabled) { update_optical_flow(); } #endif // OPTFLOW == ENABLED } // tell the scheduler one tick has passed scheduler.tick(); // run all the tasks that are due to run. Note that we only // have to call this once per loop, as the tasks are scheduled // in multiples of the main loop tick. So if they don't run on // the first call to the scheduler they won't run on a later // call until scheduler.tick() is called again uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros(); scheduler.run(time_available); }

E

//////////////////////////////////////////////////////////////////////////////// //control_acro.pde - init and run calls for acro flight mode //////////////////////////////////////////////////////////////////////////////// // acro_init - initialise acro controller static bool acro_init(bool ignore_checks) { // always successfully enter acro return true; } // acro_run - runs the acro controller // should be called at 100hz or more static void acro_run() { float target_roll, target_pitch, target_yaw; int16_t pilot_throttle_scaled; // if motors not running reset angle targets if(!motors.armed() || g.rc_3.control_in <= 0) { attitude_control.relax_bf_rate_controller(); F // relax_bf_rate_controller - ensure body-frame rate controller has zero errors to relax rate controller output void AC_AttitudeControl::relax_bf_rate_controller() { // ensure zero error in body frame rate controllers const Vector3f& gyro = _ins.get_gyro(); _rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100; } attitude_control.set_yaw_target_to_current_heading(); G // set_yaw_target_to_current_heading - sets yaw target to current heading void set_yaw_target_to_current_heading() { _angle_ef_target.z = _ahrs.yaw_sensor; } attitude_control.set_throttle_out(0, false); H // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors // provide 0 to cut motors void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost) { if (apply_angle_boost) { _motors.set_throttle(get_angle_boost(throttle_out)); }else{ _motors.set_throttle(throttle_out); // clear angle_boost for logging purposes _angle_boost = 0; } // update compass with throttle value // To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class //compass.set_throttle((float)g.rc_3.servo_out/1000.0f); } return; } // convert the input to the desired body frame rate get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); I static int16_t get_pilot_desired_throttle(int16_t throttle_control) { int16_t throttle_out; // exit immediately in the simple cases if( throttle_control == 0 || g.throttle_mid == 500) { return throttle_control; } // ensure reasonable throttle values throttle_control = constrain_int16(throttle_control,0,1000); g.throttle_mid = constrain_int16(g.throttle_mid,300,700); // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_MIDDLE) { // below the deadband throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(500-g.throttle_min)); }else if(throttle_control > THROTTLE_IN_MIDDLE) { // above the deadband throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f; }else{ // must be in the deadband throttle_out = g.throttle_mid; } return throttle_out; } // run attitude controller attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost attitude_control.set_throttle_out(pilot_throttle_scaled, false); K // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors // provide 0 to cut motors void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost) { if (apply_angle_boost) { _motors.set_throttle(get_angle_boost(throttle_out)); }else{ _motors.set_throttle(throttle_out); // clear angle_boost for logging purposes _angle_boost = 0; } // update compass with throttle value // To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class //compass.set_throttle((float)g.rc_3.servo_out/1000.0f); } } // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates // returns desired angle rates in centi-degrees-per-second static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) { // Calculate trainer mode earth frame rate command for roll float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; // calculate rate requests rate_bf_request.x = roll_in * g.acro_rp_p; rate_bf_request.y = pitch_in * g.acro_rp_p; rate_bf_request.z = yaw_in * g.acro_yaw_p; // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode if (g.acro_trainer != ACRO_TRAINER_DISABLED) { // Calculate trainer mode earth frame rate command for roll int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; // Calculate trainer mode earth frame rate command for pitch int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; // Calculate trainer mode earth frame rate command for yaw rate_ef_level.z = 0; // Calculate angle limiting earth frame rate commands if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (roll_angle > aparm.angle_max){ rate_ef_level.x -= g.acro_rp_p*(roll_angle-aparm.angle_max); }else if (roll_angle < -aparm.angle_max) { rate_ef_level.x -= g.acro_rp_p*(roll_angle+aparm.angle_max); } if (pitch_angle > aparm.angle_max){ rate_ef_level.y -= g.acro_rp_p*(pitch_angle-aparm.angle_max); }else if (pitch_angle < -aparm.angle_max) { rate_ef_level.y -= g.acro_rp_p*(pitch_angle+aparm.angle_max); } } // convert earth-frame level rates to body-frame level rates attitude_control.frame_conversion_ef_to_bf(rate_ef_level, rate_bf_level); // combine earth frame rate corrections with rate requests if (g.acro_trainer == ACRO_TRAINER_LIMITED) { rate_bf_request.x += rate_bf_level.x; rate_bf_request.y += rate_bf_level.y; rate_bf_request.z += rate_bf_level.z; }else{ acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); // Scale leveling rates by stick input rate_bf_level = rate_bf_level*acro_level_mix; // Calculate rate limit to prevent change of rate through inverted rate_limit = fabs(fabs(rate_bf_request.x)-fabs(rate_bf_level.x)); rate_bf_request.x += rate_bf_level.x; rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); // Calculate rate limit to prevent change of rate through inverted rate_limit = fabs(fabs(rate_bf_request.y)-fabs(rate_bf_level.y)); rate_bf_request.y += rate_bf_level.y; rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); // Calculate rate limit to prevent change of rate through inverted rate_limit = fabs(fabs(rate_bf_request.z)-fabs(rate_bf_level.z)); rate_bf_request.z += rate_bf_level.z; rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); } } // hand back rate request roll_out = rate_bf_request.x; pitch_out = rate_bf_request.y; yaw_out = rate_bf_request.z; }