2013-08-29 02:34:34 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2012-04-02 05:26:37 -03:00
/*
2012-08-21 23:19:52 -03:00
* AP_MotorsHeli . cpp - ArduCopter motors library
* Code by RandyMackay . DIYDrones . com
*
*/
2012-10-26 20:59:07 -03:00
# include <stdlib.h>
2015-08-11 03:28:44 -03:00
# include <AP_HAL/AP_HAL.h>
2012-04-02 05:26:37 -03:00
# include "AP_MotorsHeli.h"
2015-09-13 23:02:52 -03:00
# include <GCS_MAVLink/GCS.h>
2012-04-02 05:26:37 -03:00
2012-10-26 20:59:07 -03:00
extern const AP_HAL : : HAL & hal ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_MotorsHeli : : var_info [ ] = {
2012-08-21 23:19:52 -03:00
2014-09-18 22:54:26 -03:00
// 1 was ROL_MAX which has been replaced by CYC_MAX
2012-08-21 23:19:52 -03:00
2014-09-18 22:54:26 -03:00
// 2 was PIT_MAX which has been replaced by CYC_MAX
2012-08-21 23:19:52 -03:00
// @Param: COL_MIN
2019-12-07 18:50:33 -04:00
// @DisplayName: Minimum Collective Pitch
2017-05-15 20:21:53 -03:00
// @Description: Lowest possible servo position in PWM microseconds for the swashplate
2012-08-21 23:19:52 -03:00
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
2015-07-22 10:46:53 -03:00
AP_GROUPINFO ( " COL_MIN " , 3 , AP_MotorsHeli , _collective_min , AP_MOTORS_HELI_COLLECTIVE_MIN ) ,
2012-08-21 23:19:52 -03:00
// @Param: COL_MAX
2019-12-07 18:50:33 -04:00
// @DisplayName: Maximum Collective Pitch
2017-05-15 20:21:53 -03:00
// @Description: Highest possible servo position in PWM microseconds for the swashplate
2012-08-21 23:19:52 -03:00
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
2015-07-22 10:46:53 -03:00
AP_GROUPINFO ( " COL_MAX " , 4 , AP_MotorsHeli , _collective_max , AP_MOTORS_HELI_COLLECTIVE_MAX ) ,
2012-08-21 23:19:52 -03:00
// @Param: COL_MID
2019-12-07 18:50:33 -04:00
// @DisplayName: Zero-Thrust Collective Pitch
2017-05-15 20:21:53 -03:00
// @Description: Swash servo position in PWM microseconds corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
2012-08-21 23:19:52 -03:00
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
2015-07-22 10:46:53 -03:00
AP_GROUPINFO ( " COL_MID " , 5 , AP_MotorsHeli , _collective_mid , AP_MOTORS_HELI_COLLECTIVE_MID ) ,
2012-08-21 23:19:52 -03:00
// @Param: SV_MAN
// @DisplayName: Manual Servo Mode
2015-10-10 07:29:16 -03:00
// @Description: Manual servo override for swash set-up. Do not set this manually!
2015-10-13 14:39:58 -03:00
// @Values: 0:Disabled,1:Passthrough,2:Max collective,3:Mid collective,4:Min collective
2012-08-21 23:19:52 -03:00
// @User: Standard
2015-10-14 15:57:51 -03:00
AP_GROUPINFO ( " SV_MAN " , 6 , AP_MotorsHeli , _servo_mode , SERVO_CONTROL_MODE_AUTOMATED ) ,
2012-08-21 23:19:52 -03:00
2019-08-07 23:52:17 -03:00
// indices 7 and 8 were RSC parameters which were moved to RSC library. Do not use these indices in the future.
2012-08-21 23:19:52 -03:00
2019-04-29 23:24:32 -03:00
// index 9 was LAND_COL_MIN. Do not use this index in the future.
2013-11-06 08:39:10 -04:00
2019-08-07 23:52:17 -03:00
// indices 10-13 were RSC parameters which were moved to RSC library. Do not use these indices in the future.
2015-08-11 21:20:28 -03:00
2018-03-23 01:09:14 -03:00
// index 14 was RSC_POWER_LOW. Do not use this index in the future.
2018-03-08 13:16:31 -04:00
2018-03-23 01:09:14 -03:00
// index 15 was RSC_POWER_HIGH. Do not use this index in the future.
2015-08-07 20:58:49 -03:00
2014-09-18 22:54:26 -03:00
// @Param: CYC_MAX
2019-12-07 18:50:33 -04:00
// @DisplayName: Maximum Cyclic Pitch Angle
// @Description: Maximum cyclic pitch angle of the swash plate. There are no units to this parameter. This should be adjusted to get the desired cyclic blade pitch for the pitch and roll axes. Typically this should be 6-7 deg (measured blade pitch angle difference between stick centered and stick max deflection.
// @Range: 0 4500
2014-09-18 22:54:26 -03:00
// @Increment: 100
2019-11-17 23:33:44 -04:00
// @User: Standard
2014-09-18 22:54:26 -03:00
AP_GROUPINFO ( " CYC_MAX " , 16 , AP_MotorsHeli , _cyclic_max , AP_MOTORS_HELI_SWASH_CYCLIC_MAX ) ,
2015-10-21 20:47:24 -03:00
// @Param: SV_TEST
// @DisplayName: Boot-up Servo Test Cycles
// @Description: Number of cycles to run servo test on boot-up
// @Range: 0 10
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " SV_TEST " , 17 , AP_MotorsHeli , _servo_test , 0 ) ,
2018-03-23 01:09:14 -03:00
// index 18 was RSC_POWER_NEGC. Do not use this index in the future.
2016-06-29 23:28:34 -03:00
2019-08-07 23:52:17 -03:00
// index 19 was RSC_SLEWRATE and was moved to RSC library. Do not use this index in the future.
2018-03-23 01:09:14 -03:00
2019-08-07 23:52:17 -03:00
// indices 20 to 24 was throttle curve. Do not use this index in the future.
2019-02-03 19:19:13 -04:00
2019-08-07 23:52:17 -03:00
// @Group: RSC_
2019-02-14 20:28:48 -04:00
// @Path: AP_MotorsHeli_RSC.cpp
2019-08-07 23:52:17 -03:00
AP_SUBGROUPINFO ( _main_rotor , " RSC_ " , 25 , AP_MotorsHeli , AP_MotorsHeli_RSC ) ,
2019-02-03 19:19:13 -04:00
2012-04-02 05:26:37 -03:00
AP_GROUPEND
} ;
2013-11-04 07:53:27 -04:00
//
// public methods
//
2012-04-02 05:26:37 -03:00
// init
2016-12-14 01:46:42 -04:00
void AP_MotorsHeli : : init ( motor_frame_class frame_class , motor_frame_type frame_type )
2012-04-02 05:26:37 -03:00
{
2019-06-22 22:40:26 -03:00
// remember frame class and type
2017-09-30 03:30:02 -03:00
_frame_type = frame_type ;
2019-06-22 22:40:26 -03:00
_frame_class = frame_class ;
2018-03-08 13:16:31 -04:00
2012-08-21 23:19:52 -03:00
// set update rate
set_update_rate ( _speed_hz ) ;
2013-11-04 07:53:27 -04:00
2015-10-21 20:47:24 -03:00
// load boot-up servo test cycles into counter to be consumed
_servo_test_cycle_counter = _servo_test ;
2015-08-12 12:41:40 -03:00
// ensure inputs are not passed through to servos on start-up
2015-10-14 15:57:51 -03:00
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED ;
2013-11-04 07:53:27 -04:00
2016-02-05 21:42:10 -04:00
// initialise radio passthrough for collective to middle
_throttle_radio_passthrough = 0.5f ;
2015-08-12 14:22:39 -03:00
// initialise Servo/PWM ranges and endpoints
2017-01-03 05:56:57 -04:00
if ( ! init_outputs ( ) ) {
// don't set initialised_ok
return ;
}
2015-08-12 12:41:40 -03:00
// calculate all scalars
calculate_scalars ( ) ;
2016-12-14 01:46:42 -04:00
// record successful initialisation if what we setup was the desired frame_class
_flags . initialised_ok = ( frame_class = = MOTOR_FRAME_HELI ) ;
2018-12-28 02:32:05 -04:00
// set flag to true so targets are initialized once aircraft is armed for first time
_heliflags . init_targets_on_arming = true ;
2016-12-14 01:46:42 -04:00
}
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void AP_MotorsHeli : : set_frame_class_and_type ( motor_frame_class frame_class , motor_frame_type frame_type )
{
_flags . initialised_ok = ( frame_class = = MOTOR_FRAME_HELI ) ;
2012-04-02 05:26:37 -03:00
}
2015-08-12 14:22:39 -03:00
// output_min - sets servos to neutral point with motors stopped
void AP_MotorsHeli : : output_min ( )
{
// move swash to mid
2016-02-02 08:31:44 -04:00
move_actuators ( 0.0f , 0.0f , 0.5f , 0.0f ) ;
2015-08-12 14:22:39 -03:00
update_motor_control ( ROTOR_CONTROL_STOP ) ;
// override limits flags
2019-07-27 02:37:31 -03:00
limit . roll = true ;
limit . pitch = true ;
2015-08-12 14:22:39 -03:00
limit . yaw = true ;
limit . throttle_lower = true ;
limit . throttle_upper = false ;
}
2015-05-21 15:33:18 -03:00
// output - sends commands to the servos
void AP_MotorsHeli : : output ( )
{
// update throttle filter
update_throttle_filter ( ) ;
2018-12-28 02:32:05 -04:00
// run spool logic
output_logic ( ) ;
2015-05-21 15:33:18 -03:00
if ( _flags . armed ) {
2020-08-03 23:38:11 -03:00
//block servo_test from happening at disarm
_servo_test_cycle_counter = 0 ;
2015-11-14 18:14:03 -04:00
calculate_armed_scalars ( ) ;
2015-05-21 15:33:18 -03:00
if ( ! _flags . interlock ) {
output_armed_zero_throttle ( ) ;
} else {
2016-01-18 01:06:54 -04:00
output_armed_stabilizing ( ) ;
2015-05-21 15:33:18 -03:00
}
} else {
output_disarmed ( ) ;
}
2018-12-28 02:32:05 -04:00
output_to_motors ( ) ;
2015-05-21 15:33:18 -03:00
} ;
2015-08-12 14:22:39 -03:00
// sends commands to the motors
void AP_MotorsHeli : : output_armed_stabilizing ( )
{
// if manual override active after arming, deactivate it and reinitialize servos
2015-10-14 15:57:51 -03:00
if ( _servo_mode ! = SERVO_CONTROL_MODE_AUTOMATED ) {
2015-08-12 14:22:39 -03:00
reset_flight_controls ( ) ;
}
2016-01-21 22:10:36 -04:00
move_actuators ( _roll_in , _pitch_in , get_throttle ( ) , _yaw_in ) ;
2015-08-12 14:22:39 -03:00
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli : : output_armed_zero_throttle ( )
{
// if manual override active after arming, deactivate it and reinitialize servos
2015-10-14 15:57:51 -03:00
if ( _servo_mode ! = SERVO_CONTROL_MODE_AUTOMATED ) {
2015-08-12 14:22:39 -03:00
reset_flight_controls ( ) ;
}
2016-01-21 22:10:36 -04:00
move_actuators ( _roll_in , _pitch_in , get_throttle ( ) , _yaw_in ) ;
2015-08-12 14:22:39 -03:00
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli : : output_disarmed ( )
{
2015-10-21 20:47:24 -03:00
if ( _servo_test_cycle_counter > 0 ) {
2020-08-03 23:38:11 -03:00
//set servo_test_flag
_heliflags . servo_test_running = true ;
2015-10-21 20:47:24 -03:00
// perform boot-up servo test cycle if enabled
servo_test ( ) ;
} else {
2020-08-03 23:38:11 -03:00
//set servo_test flag
_heliflags . servo_test_running = false ;
2015-10-21 20:47:24 -03:00
// manual override (i.e. when setting up swash)
switch ( _servo_mode ) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH :
// pass pilot commands straight through to swash
2016-01-21 22:10:36 -04:00
_roll_in = _roll_radio_passthrough ;
_pitch_in = _pitch_radio_passthrough ;
2016-02-03 07:57:44 -04:00
_throttle_filter . reset ( _throttle_radio_passthrough ) ;
2016-01-21 22:10:36 -04:00
_yaw_in = _yaw_radio_passthrough ;
2015-10-21 20:47:24 -03:00
break ;
case SERVO_CONTROL_MODE_MANUAL_CENTER :
// fixate mid collective
2016-01-21 22:10:36 -04:00
_roll_in = 0.0f ;
_pitch_in = 0.0f ;
2016-02-05 21:39:20 -04:00
_throttle_filter . reset ( _collective_mid_pct ) ;
2016-01-21 22:10:36 -04:00
_yaw_in = 0.0f ;
2015-10-21 20:47:24 -03:00
break ;
case SERVO_CONTROL_MODE_MANUAL_MAX :
// fixate max collective
2016-01-21 22:10:36 -04:00
_roll_in = 0.0f ;
_pitch_in = 0.0f ;
2016-01-06 05:06:15 -04:00
_throttle_filter . reset ( 1.0f ) ;
2019-06-22 22:40:26 -03:00
if ( _frame_class = = MOTOR_FRAME_HELI_DUAL | |
_frame_class = = MOTOR_FRAME_HELI_QUAD ) {
_yaw_in = 0 ;
} else {
_yaw_in = 1 ;
}
2015-10-21 20:47:24 -03:00
break ;
case SERVO_CONTROL_MODE_MANUAL_MIN :
// fixate min collective
2016-01-21 22:10:36 -04:00
_roll_in = 0.0f ;
_pitch_in = 0.0f ;
2016-01-06 05:06:15 -04:00
_throttle_filter . reset ( 0.0f ) ;
2019-06-22 22:40:26 -03:00
if ( _frame_class = = MOTOR_FRAME_HELI_DUAL | |
_frame_class = = MOTOR_FRAME_HELI_QUAD ) {
_yaw_in = 0 ;
} else {
_yaw_in = - 1 ;
}
2015-10-21 20:47:24 -03:00
break ;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE :
// use servo_test function from child classes
servo_test ( ) ;
break ;
default :
// no manual override
break ;
}
2015-08-12 14:22:39 -03:00
}
// ensure swash servo endpoints haven't been moved
init_outputs ( ) ;
// continuously recalculate scalars to allow setup
calculate_scalars ( ) ;
// helicopters always run stabilizing flight controls
2016-01-21 22:10:36 -04:00
move_actuators ( _roll_in , _pitch_in , get_throttle ( ) , _yaw_in ) ;
2018-12-28 02:32:05 -04:00
}
2015-08-12 14:22:39 -03:00
2018-12-28 02:32:05 -04:00
// run spool logic
void AP_MotorsHeli : : output_logic ( )
{
// force desired and current spool mode if disarmed and armed with interlock enabled
if ( _flags . armed ) {
if ( ! _flags . interlock ) {
2019-04-09 09:15:45 -03:00
_spool_desired = DesiredSpoolState : : GROUND_IDLE ;
2018-12-28 02:32:05 -04:00
} else {
_heliflags . init_targets_on_arming = false ;
}
} else {
_heliflags . init_targets_on_arming = true ;
2019-04-09 09:15:45 -03:00
_spool_desired = DesiredSpoolState : : SHUT_DOWN ;
_spool_state = SpoolState : : SHUT_DOWN ;
2018-12-28 02:32:05 -04:00
}
2019-04-09 09:15:45 -03:00
switch ( _spool_state ) {
case SpoolState : : SHUT_DOWN :
2018-12-28 02:32:05 -04:00
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// make sure the motors are spooling in the correct direction
2019-04-09 09:15:45 -03:00
if ( _spool_desired ! = DesiredSpoolState : : SHUT_DOWN ) {
_spool_state = SpoolState : : GROUND_IDLE ;
2018-12-28 02:32:05 -04:00
break ;
}
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : GROUND_IDLE : {
2018-12-28 02:32:05 -04:00
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
2019-04-09 09:15:45 -03:00
if ( _spool_desired = = DesiredSpoolState : : SHUT_DOWN ) {
_spool_state = SpoolState : : SHUT_DOWN ;
} else if ( _spool_desired = = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_UP ;
2018-12-28 02:32:05 -04:00
} else { // _spool_desired == GROUND_IDLE
}
break ;
}
2019-04-09 09:15:45 -03:00
case SpoolState : : SPOOLING_UP :
2018-12-28 02:32:05 -04:00
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// make sure the motors are spooling in the correct direction
2019-04-09 09:15:45 -03:00
if ( _spool_desired ! = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_DOWN ;
2018-12-28 02:32:05 -04:00
break ;
}
if ( _heliflags . rotor_runup_complete ) {
2019-04-09 09:15:45 -03:00
_spool_state = SpoolState : : THROTTLE_UNLIMITED ;
2018-12-28 02:32:05 -04:00
}
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : THROTTLE_UNLIMITED :
2018-12-28 02:32:05 -04:00
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// make sure the motors are spooling in the correct direction
2019-04-09 09:15:45 -03:00
if ( _spool_desired ! = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_DOWN ;
2018-12-28 02:32:05 -04:00
break ;
}
break ;
2019-04-09 09:15:45 -03:00
case SpoolState : : SPOOLING_DOWN :
2018-12-28 02:32:05 -04:00
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// make sure the motors are spooling in the correct direction
2019-04-09 09:15:45 -03:00
if ( _spool_desired = = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_UP ;
2018-12-28 02:32:05 -04:00
break ;
}
if ( ! rotor_speed_above_critical ( ) ) {
2019-04-09 09:15:45 -03:00
_spool_state = SpoolState : : GROUND_IDLE ;
2018-12-28 02:32:05 -04:00
}
break ;
}
2015-08-12 14:22:39 -03:00
}
2015-07-08 14:14:49 -03:00
// parameter_check - check if helicopter specific parameters are sensible
2015-09-13 23:02:52 -03:00
bool AP_MotorsHeli : : parameter_check ( bool display_msg ) const
2015-07-08 14:14:49 -03:00
{
2019-08-07 23:52:17 -03:00
// returns false if RSC Mode is not set to a valid control mode
if ( _main_rotor . _rsc_mode . get ( ) < = ( int8_t ) ROTOR_CONTROL_MODE_DISABLED | | _main_rotor . _rsc_mode . get ( ) > ( int8_t ) ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT ) {
2015-09-13 23:02:52 -03:00
if ( display_msg ) {
2019-08-07 23:52:17 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_RSC_MODE invalid " ) ;
2015-09-13 23:02:52 -03:00
}
2015-07-08 14:14:49 -03:00
return false ;
}
2019-08-07 23:52:17 -03:00
// returns false if rsc_setpoint is out of range
if ( _main_rotor . _rsc_setpoint . get ( ) > 100 | | _main_rotor . _rsc_setpoint . get ( ) < 10 ) {
2015-09-13 23:02:52 -03:00
if ( display_msg ) {
2019-08-07 23:52:17 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_RSC_SETPOINT out of range " ) ;
2015-09-13 23:02:52 -03:00
}
2015-08-10 18:29:17 -03:00
return false ;
}
2019-08-07 23:52:17 -03:00
// returns false if idle output is out of range
if ( _main_rotor . _idle_output . get ( ) > 100 | | _main_rotor . _idle_output . get ( ) < 0 ) {
2015-09-13 23:02:52 -03:00
if ( display_msg ) {
2019-08-07 23:52:17 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_RSC_IDLE out of range " ) ;
2015-09-13 23:02:52 -03:00
}
2015-08-10 18:29:17 -03:00
return false ;
}
2019-08-07 23:52:17 -03:00
// returns false if _rsc_critical is not between 0 and 100
if ( _main_rotor . _critical_speed . get ( ) > 100 | | _main_rotor . _critical_speed . get ( ) < 0 ) {
2015-09-13 23:02:52 -03:00
if ( display_msg ) {
2019-08-07 23:52:17 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_RSC_CRITICAL out of range " ) ;
}
return false ;
}
// returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate
if ( _main_rotor . _runup_time . get ( ) < = _main_rotor . _ramp_time . get ( ) ) {
if ( display_msg ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: H_RUNUP_TIME too small " ) ;
2015-09-13 23:02:52 -03:00
}
2015-08-10 18:29:17 -03:00
return false ;
}
2015-07-08 14:14:49 -03:00
// all other cases parameters are OK
return true ;
}
2015-07-22 05:14:05 -03:00
// reset_swash_servo
2018-07-11 16:33:00 -03:00
void AP_MotorsHeli : : reset_swash_servo ( SRV_Channel : : Aux_servo_function_t function )
2015-07-22 05:14:05 -03:00
{
2018-07-11 16:33:00 -03:00
// outputs are defined on a -500 to 500 range for swash servos
SRV_Channels : : set_range ( function , 1000 ) ;
2015-08-12 12:41:40 -03:00
// swash servos always use full endpoints as restricting them would lead to scaling errors
2018-07-11 16:33:00 -03:00
SRV_Channels : : set_output_min_max ( function , 1000 , 2000 ) ;
2015-07-22 05:14:05 -03:00
}
2015-05-21 15:19:25 -03:00
// update the throttle input filter
void AP_MotorsHeli : : update_throttle_filter ( )
{
_throttle_filter . apply ( _throttle_in , 1.0f / _loop_rate ) ;
2016-03-22 23:29:32 -03:00
// constrain filtered throttle
if ( _throttle_filter . get ( ) < 0.0f ) {
_throttle_filter . reset ( 0.0f ) ;
}
if ( _throttle_filter . get ( ) > 1.0f ) {
_throttle_filter . reset ( 1.0f ) ;
}
2015-05-14 22:00:46 -03:00
}
2015-08-12 14:22:39 -03:00
// reset_flight_controls - resets all controls and scalars to flight status
void AP_MotorsHeli : : reset_flight_controls ( )
{
2015-10-14 15:57:51 -03:00
_servo_mode = SERVO_CONTROL_MODE_AUTOMATED ;
2015-08-12 14:22:39 -03:00
init_outputs ( ) ;
calculate_scalars ( ) ;
}
2018-04-21 00:17:50 -03:00
2018-07-11 16:33:00 -03:00
// convert input in -1 to +1 range to pwm output for swashplate servo.
// The value 0 corresponds to the trim value of the servo. Swashplate
// servo travel range is fixed to 1000 pwm and therefore the input is
// multiplied by 500 to get PWM output.
void AP_MotorsHeli : : rc_write_swash ( uint8_t chan , float swash_in )
2018-04-21 00:17:50 -03:00
{
2018-07-11 16:33:00 -03:00
uint16_t pwm = ( uint16_t ) ( 1500 + 500 * swash_in ) ;
SRV_Channel : : Aux_servo_function_t function = SRV_Channels : : get_motor_function ( chan ) ;
SRV_Channels : : set_output_pwm_trimmed ( function , pwm ) ;
2018-04-21 00:17:50 -03:00
}