void pwm_servos_calibrate_esc(const servos_t* servos) { int16_t i; servos_t speed_controller = *servos; for(i = 0;i < 4; ++i) { speed_controller.servo[i].value = servos->servo[i].max; } pwm_servos_write_to_hardware(&speed_controller); delay_ms(2000); servos_set_value_failsafe(&speed_controller); pwm_servos_write_to_hardware(&speed_controller); }
task_return_t tasks_run_stabilisation(void* arg) { tasks_run_imu_update(0); mav_mode_t mode = central_data->state.mav_mode; if( mode.ARMED == ARMED_ON ) { if ( mode.AUTO == AUTO_ON ) { central_data->controls = central_data->controls_nav; central_data->controls.control_mode = VELOCITY_COMMAND_MODE; // if no waypoints are set, we do position hold therefore the yaw mode is absolute if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP))) { central_data->controls.yaw_mode = YAW_COORDINATED; } else { central_data->controls.yaw_mode = YAW_ABSOLUTE; } if (central_data->state.in_the_air || central_data->navigation.auto_takeoff) { stabilisation_copter_cascade_stabilise(¢ral_data->stabilisation_copter); } } else if ( mode.GUIDED == GUIDED_ON ) { central_data->controls = central_data->controls_nav; central_data->controls.control_mode = VELOCITY_COMMAND_MODE; if ((central_data->state.mav_state == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == FLY_TO_HOME_WP)) { central_data->controls.yaw_mode = YAW_COORDINATED; } else { central_data->controls.yaw_mode = YAW_ABSOLUTE; } if (central_data->state.in_the_air || central_data->navigation.auto_takeoff) { stabilisation_copter_cascade_stabilise(¢ral_data->stabilisation_copter); } } else if ( mode.STABILISE == STABILISE_ON ) { if (central_data->state.remote_active == 1) { remote_get_velocity_vector_from_remote(¢ral_data->remote, ¢ral_data->controls); } else { joystick_parsing_get_velocity_vector_from_joystick(¢ral_data->joystick_parsing,¢ral_data->controls); } central_data->controls.control_mode = VELOCITY_COMMAND_MODE; central_data->controls.yaw_mode = YAW_RELATIVE; if (central_data->state.in_the_air || central_data->navigation.auto_takeoff) { stabilisation_copter_cascade_stabilise(¢ral_data->stabilisation_copter); } } else if ( mode.MANUAL == MANUAL_ON ) { if (central_data->state.remote_active == 1) { remote_get_command_from_remote(¢ral_data->remote, ¢ral_data->controls); } else { joystick_parsing_get_attitude_command_from_joystick(¢ral_data->joystick_parsing,¢ral_data->controls); } central_data->controls.control_mode = ATTITUDE_COMMAND_MODE; central_data->controls.yaw_mode=YAW_RELATIVE; stabilisation_copter_cascade_stabilise(¢ral_data->stabilisation_copter); } else { servos_set_value_failsafe( ¢ral_data->servos ); } } else { servos_set_value_failsafe( ¢ral_data->servos ); } // !!! -- for safety, this should remain the only place where values are written to the servo outputs! --- !!! if ( mode.HIL == HIL_OFF ) { pwm_servos_write_to_hardware( ¢ral_data->servos ); } return TASK_RUN_SUCCESS; }
task_return_t tasks_run_stabilisation_quaternion(void* arg) { tasks_run_imu_update(0); mav_mode_t mode = central_data->state.mav_mode; if( mode.ARMED == ARMED_OFF ) { // Set command to current heading central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2]; servos_set_value_failsafe( ¢ral_data->servos ); } else if( mode.AUTO == AUTO_ON ) { vector_field_waypoint_update( ¢ral_data->vector_field_waypoint ); velocity_controller_copter_update( ¢ral_data->velocity_controller ); attitude_controller_update( ¢ral_data->attitude_controller ); servos_mix_quadcopter_diag_update( ¢ral_data->servo_mix ); } else if( mode.MANUAL == MANUAL_ON && mode.GUIDED == GUIDED_ON ) { remote_get_velocity_vector_from_remote(¢ral_data->remote, ¢ral_data->controls); central_data->command.velocity.xyz[X] = central_data->controls.tvel[X]; central_data->command.velocity.xyz[Y] = central_data->controls.tvel[Y]; central_data->command.velocity.xyz[Z] = central_data->controls.tvel[Z]; central_data->command.velocity.mode = VELOCITY_COMMAND_MODE_GLOBAL; velocity_controller_copter_update( ¢ral_data->velocity_controller ); attitude_controller_update( ¢ral_data->attitude_controller ); servos_mix_quadcopter_diag_update( ¢ral_data->servo_mix ); } else if( mode.MANUAL == MANUAL_ON && mode.STABILISE == STABILISE_ON ) { remote_get_command_from_remote(¢ral_data->remote, ¢ral_data->controls); central_data->command.attitude.rpy[0] = central_data->controls.rpy[0]; central_data->command.attitude.rpy[1] = central_data->controls.rpy[1]; central_data->command.attitude.rpy[2] += 0.02 * central_data->controls.rpy[2]; central_data->command.attitude.mode = ATTITUDE_COMMAND_MODE_RPY; central_data->command.thrust.thrust = central_data->controls.thrust; attitude_controller_update( ¢ral_data->attitude_controller ); // altitude // if( mode.GUIDED == GUIDED_ON ) // { // float delta_h = 1.0 - central_data->sonar_i2cxl.data.current_distance; // float k_h = 0.2f; // central_data->command.thrust.thrust += k_h * delta_h; // } servos_mix_quadcopter_diag_update( ¢ral_data->servo_mix ); } else { servos_set_value_failsafe( ¢ral_data->servos ); } // !!! -- for safety, this should remain the only place where values are written to the servo outputs! --- !!! if ( mode.ARMED == ARMED_ON && mode.HIL == HIL_OFF ) { pwm_servos_write_to_hardware( ¢ral_data->servos ); } return TASK_RUN_SUCCESS; }