2017-02-05 22:15:19 -04: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/>.
*/
/*
SRV_Channel . cpp - object to separate input and output channel
ranges , trim and reversal
*/
# include <AP_HAL/AP_HAL.h>
# include <AP_Math/AP_Math.h>
# include <AP_Vehicle/AP_Vehicle.h>
# include "SRV_Channel.h"
2018-03-09 11:35:43 -04:00
# if HAL_WITH_UAVCAN
2018-07-04 16:04:30 -03:00
# include <AP_BoardConfig/AP_BoardConfig_CAN.h>
# include <AP_UAVCAN/AP_UAVCAN.h>
// To be replaced with macro saying if KDECAN library is included
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
# include <AP_KDECAN/AP_KDECAN.h>
# endif
2019-01-15 01:24:20 -04:00
# include <AP_ToshibaCAN/AP_ToshibaCAN.h>
2019-12-09 00:31:29 -04:00
# include <AP_PiccoloCAN/AP_PiccoloCAN.h>
2018-03-09 11:35:43 -04:00
# endif
2017-02-05 22:15:19 -04:00
extern const AP_HAL : : HAL & hal ;
SRV_Channel * SRV_Channels : : channels ;
2019-02-10 00:59:44 -04:00
SRV_Channels * SRV_Channels : : _singleton ;
2017-11-02 23:27:29 -03:00
AP_Volz_Protocol * SRV_Channels : : volz_ptr ;
2017-11-22 13:40:44 -04:00
AP_SBusOut * SRV_Channels : : sbus_ptr ;
2018-12-18 23:28:35 -04:00
AP_RobotisServo * SRV_Channels : : robotis_ptr ;
2018-04-01 22:16:44 -03:00
# if HAL_SUPPORT_RCOUT_SERIAL
2018-03-24 19:23:03 -03:00
AP_BLHeli * SRV_Channels : : blheli_ptr ;
2018-04-01 22:16:44 -03:00
# endif
2018-04-01 03:01:37 -03:00
uint16_t SRV_Channels : : disabled_mask ;
2018-11-09 06:26:38 -04:00
uint16_t SRV_Channels : : digital_mask ;
uint16_t SRV_Channels : : reversible_mask ;
2017-11-02 23:27:29 -03:00
2017-02-05 22:15:19 -04:00
bool SRV_Channels : : disabled_passthrough ;
bool SRV_Channels : : initialised ;
2019-01-20 21:36:10 -04:00
bool SRV_Channels : : emergency_stop ;
2019-04-11 09:13:23 -03:00
Bitmask < SRV_Channel : : k_nr_aux_servo_functions > SRV_Channels : : function_mask ;
2017-02-05 22:15:19 -04:00
SRV_Channels : : srv_function SRV_Channels : : functions [ SRV_Channel : : k_nr_aux_servo_functions ] ;
const AP_Param : : GroupInfo SRV_Channels : : var_info [ ] = {
// @Group: 1_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 0 ] , " 1_ " , 1 , SRV_Channels , SRV_Channel ) ,
// @Group: 2_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 1 ] , " 2_ " , 2 , SRV_Channels , SRV_Channel ) ,
// @Group: 3_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 2 ] , " 3_ " , 3 , SRV_Channels , SRV_Channel ) ,
// @Group: 4_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 3 ] , " 4_ " , 4 , SRV_Channels , SRV_Channel ) ,
// @Group: 5_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 4 ] , " 5_ " , 5 , SRV_Channels , SRV_Channel ) ,
// @Group: 6_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 5 ] , " 6_ " , 6 , SRV_Channels , SRV_Channel ) ,
// @Group: 7_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 6 ] , " 7_ " , 7 , SRV_Channels , SRV_Channel ) ,
// @Group: 8_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 7 ] , " 8_ " , 8 , SRV_Channels , SRV_Channel ) ,
// @Group: 9_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 8 ] , " 9_ " , 9 , SRV_Channels , SRV_Channel ) ,
// @Group: 10_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 9 ] , " 10_ " , 10 , SRV_Channels , SRV_Channel ) ,
// @Group: 11_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 10 ] , " 11_ " , 11 , SRV_Channels , SRV_Channel ) ,
// @Group: 12_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 11 ] , " 12_ " , 12 , SRV_Channels , SRV_Channel ) ,
// @Group: 13_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 12 ] , " 13_ " , 13 , SRV_Channels , SRV_Channel ) ,
// @Group: 14_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 13 ] , " 14_ " , 14 , SRV_Channels , SRV_Channel ) ,
// @Group: 15_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 14 ] , " 15_ " , 15 , SRV_Channels , SRV_Channel ) ,
// @Group: 16_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO ( obj_channels [ 15 ] , " 16_ " , 16 , SRV_Channels , SRV_Channel ) ,
2017-07-25 00:23:09 -03:00
2017-02-05 22:15:19 -04:00
// @Param: _AUTO_TRIM
// @DisplayName: Automatic servo trim
2020-01-19 15:50:25 -04:00
// @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights. The automatic trim won't go more than 20% away from a centered trim.
2017-02-05 22:15:19 -04:00
// @Values: 0:Disable,1:Enable
// @User: Advanced
2017-02-07 19:48:39 -04:00
AP_GROUPINFO_FRAME ( " _AUTO_TRIM " , 17 , SRV_Channels , auto_trim , 0 , AP_PARAM_FRAME_PLANE ) ,
2017-04-02 11:55:50 -03:00
2017-07-25 00:15:00 -03:00
// @Param: _RATE
// @DisplayName: Servo default output rate
// @Description: This sets the default output rate in Hz for all outputs.
2017-07-16 20:19:45 -03:00
// @Range: 25 400
// @User: Advanced
// @Units: Hz
2017-07-25 00:15:00 -03:00
AP_GROUPINFO ( " _RATE " , 18 , SRV_Channels , default_rate , 50 ) ,
2017-07-25 00:23:09 -03:00
2017-12-08 17:08:00 -04:00
// @Group: _VOLZ_
2017-11-02 23:27:29 -03:00
// @Path: ../AP_Volz_Protocol/AP_Volz_Protocol.cpp
AP_SUBGROUPINFO ( volz , " _VOLZ_ " , 19 , SRV_Channels , AP_Volz_Protocol ) ,
2017-12-08 17:08:00 -04:00
// @Group: _SBUS_
2017-11-22 13:40:44 -04:00
// @Path: ../AP_SBusOut/AP_SBusOut.cpp
AP_SUBGROUPINFO ( sbus , " _SBUS_ " , 20 , SRV_Channels , AP_SBusOut ) ,
2018-04-01 22:16:44 -03:00
# if HAL_SUPPORT_RCOUT_SERIAL
2018-03-24 19:23:03 -03:00
// @Group: _BLH_
// @Path: ../AP_BLHeli/AP_BLHeli.cpp
AP_SUBGROUPINFO ( blheli , " _BLH_ " , 21 , SRV_Channels , AP_BLHeli ) ,
2018-04-01 22:16:44 -03:00
# endif
2018-07-18 03:27:38 -03:00
2018-12-18 23:28:35 -04:00
// @Group: _ROB_
// @Path: ../AP_RobotisServo/AP_RobotisServo.cpp
AP_SUBGROUPINFO ( robotis , " _ROB_ " , 22 , SRV_Channels , AP_RobotisServo ) ,
2017-02-05 22:15:19 -04:00
AP_GROUPEND
} ;
/*
constructor
*/
SRV_Channels : : SRV_Channels ( void )
{
2019-02-10 00:59:44 -04:00
_singleton = this ;
2017-02-05 22:15:19 -04:00
channels = obj_channels ;
2017-07-25 00:23:09 -03:00
2017-02-05 22:15:19 -04:00
// set defaults from the parameter table
AP_Param : : setup_object_defaults ( this , var_info ) ;
// setup ch_num on channels
for ( uint8_t i = 0 ; i < NUM_SERVO_CHANNELS ; i + + ) {
channels [ i ] . ch_num = i ;
}
2017-11-02 23:27:29 -03:00
volz_ptr = & volz ;
2017-11-22 13:40:44 -04:00
sbus_ptr = & sbus ;
2018-12-18 23:28:35 -04:00
robotis_ptr = & robotis ;
2018-04-01 22:16:44 -03:00
# if HAL_SUPPORT_RCOUT_SERIAL
2018-03-24 19:23:03 -03:00
blheli_ptr = & blheli ;
2018-04-01 22:16:44 -03:00
# endif
2017-02-05 22:15:19 -04:00
}
/*
save adjusted trims
*/
void SRV_Channels : : save_trim ( void )
{
for ( uint8_t i = 0 ; i < NUM_SERVO_CHANNELS ; i + + ) {
if ( trimmed_mask & ( 1U < < i ) ) {
channels [ i ] . servo_trim . set_and_save ( channels [ i ] . servo_trim . get ( ) ) ;
}
}
trimmed_mask = 0 ;
}
2018-08-22 04:18:36 -03:00
void SRV_Channels : : setup_failsafe_trim_all_non_motors ( void )
2017-02-05 22:15:19 -04:00
{
for ( uint8_t i = 0 ; i < NUM_SERVO_CHANNELS ; i + + ) {
2018-08-22 04:18:36 -03:00
if ( ! SRV_Channel : : is_motor ( channels [ i ] . get_function ( ) ) ) {
hal . rcout - > set_failsafe_pwm ( 1U < < channels [ i ] . ch_num , channels [ i ] . servo_trim ) ;
}
2017-02-05 22:15:19 -04:00
}
}
/*
run calc_pwm for all channels
*/
void SRV_Channels : : calc_pwm ( void )
{
for ( uint8_t i = 0 ; i < NUM_SERVO_CHANNELS ; i + + ) {
channels [ i ] . calc_pwm ( functions [ channels [ i ] . function ] . output_scaled ) ;
}
}
2017-04-17 03:53:05 -03:00
// set output value for a specific function channel as a pwm value
void SRV_Channels : : set_output_pwm_chan ( uint8_t chan , uint16_t value )
{
if ( chan < NUM_SERVO_CHANNELS ) {
channels [ chan ] . set_output_pwm ( value ) ;
}
}
2017-11-02 23:27:29 -03:00
/*
wrapper around hal . rcout - > cork ( )
*/
void SRV_Channels : : cork ( )
{
2018-03-09 11:35:43 -04:00
hal . rcout - > cork ( ) ;
2017-11-02 23:27:29 -03:00
}
/*
wrapper around hal . rcout - > push ( )
*/
void SRV_Channels : : push ( )
{
hal . rcout - > push ( ) ;
2018-07-18 03:27:38 -03:00
2017-11-02 23:27:29 -03:00
// give volz library a chance to update
volz_ptr - > update ( ) ;
2017-11-22 13:40:44 -04:00
// give sbus library a chance to update
sbus_ptr - > update ( ) ;
2018-04-01 22:16:44 -03:00
2018-12-18 23:28:35 -04:00
// give robotis library a chance to update
robotis_ptr - > update ( ) ;
2018-04-01 22:16:44 -03:00
# if HAL_SUPPORT_RCOUT_SERIAL
// give blheli telemetry a chance to update
blheli_ptr - > update_telemetry ( ) ;
# endif
2018-03-09 11:35:43 -04:00
# if HAL_WITH_UAVCAN
2018-07-04 16:04:30 -03:00
// push outputs to CAN
2018-07-18 03:27:38 -03:00
uint8_t can_num_drivers = AP : : can ( ) . get_num_drivers ( ) ;
for ( uint8_t i = 0 ; i < can_num_drivers ; i + + ) {
2018-07-04 16:04:30 -03:00
switch ( AP : : can ( ) . get_protocol_type ( i ) ) {
case AP_BoardConfig_CAN : : Protocol_Type_UAVCAN : {
AP_UAVCAN * ap_uavcan = AP_UAVCAN : : get_uavcan ( i ) ;
if ( ap_uavcan = = nullptr ) {
continue ;
}
ap_uavcan - > SRV_push_servos ( ) ;
break ;
}
case AP_BoardConfig_CAN : : Protocol_Type_KDECAN : {
// To be replaced with macro saying if KDECAN library is included
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
AP_KDECAN * ap_kdecan = AP_KDECAN : : get_kdecan ( i ) ;
if ( ap_kdecan = = nullptr ) {
continue ;
}
ap_kdecan - > update ( ) ;
break ;
# endif
}
2019-01-15 01:24:20 -04:00
case AP_BoardConfig_CAN : : Protocol_Type_ToshibaCAN : {
AP_ToshibaCAN * ap_tcan = AP_ToshibaCAN : : get_tcan ( i ) ;
if ( ap_tcan = = nullptr ) {
continue ;
}
ap_tcan - > update ( ) ;
break ;
}
2019-12-09 00:31:29 -04:00
# if HAL_PICCOLO_CAN_ENABLE
case AP_BoardConfig_CAN : : Protocol_Type_PiccoloCAN : {
AP_PiccoloCAN * ap_pcan = AP_PiccoloCAN : : get_pcan ( i ) ;
if ( ap_pcan = = nullptr ) {
continue ;
}
ap_pcan - > update ( ) ;
break ;
}
# endif
2018-07-04 16:04:30 -03:00
case AP_BoardConfig_CAN : : Protocol_Type_None :
default :
break ;
2018-03-09 11:35:43 -04:00
}
}
# endif // HAL_WITH_UAVCAN
}