Ardupilot的植保喷洒作业的控制代码讲解
目录
- 目录
- 摘要
- 1.植保喷洒初始化设置
- 2.喷洒作业更新运行
摘要
本文主要记录自己学习ardupilot的植保喷洒作业的过程。ardupilot要想进行植保喷洒，必须把水泵连接到SERVO上面，我这里使用的是SERVO9，也就是排针的第九个引脚，如果所示。通过遥控器的外部开关的调节来控制定时器输出PWM大小，进行植保作业。查阅网上资料对这块基本没有讲解，都是基本使用。这里对这部分代码进行讲解下，有不对的地方欢迎批评指正！！！
这里主要讲解两种方式来实现喷洒设置：
（1）连动方式（一个遥控开关控制）
（2）遥控器通道直接控制（两个开关控制）
1.植保喷洒初始化设置
用到的主要代码在这里
（1）进行植保作业喷洒需要注意的参数设置
在massionplanner中主要要设置的参数是：
这里需要说明：一定要弄清楚：选择的遥控器通道和servo输出控制水泵不是一回事，特别注意！！!需要思考就是怎么实现遥控器通道映射到可以控制servo输出控制水泵。
一定要打开servo9的功能函数设置，因为每个servo9上面有很多功能
对应的参数设置：
typedef enum
{
k_none = 0, ///< disabled
k_manual = 1, ///< manual, just pass-thru the RC in signal
k_flap = 2, ///< flap
k_flap_auto = 3, ///< flap automated
k_aileron = 4, ///< aileron
k_unused1 = 5, ///< unused function
k_mount_pan = 6, ///< mount yaw (pan)
k_mount_tilt = 7, ///< mount pitch (tilt)
k_mount_roll = 8, ///< mount roll
k_mount_open = 9, ///< mount open (deploy) / close (retract)
k_cam_trigger = 10, ///< camera trigger
k_egg_drop = 11, ///< egg drop
k_mount2_pan = 12, ///< mount2 yaw (pan)
k_mount2_tilt = 13, ///< mount2 pitch (tilt)
k_mount2_roll = 14, ///< mount2 roll
k_mount2_open = 15, ///< mount2 open (deploy) / close (retract)
k_dspoiler1 = 16, ///< differential spoiler 1 (left wing)
k_dspoiler2 = 17, ///< differential spoiler 2 (right wing)
k_aileron_with_input = 18, ///< aileron, with rc input
k_elevator = 19, ///< elevator
k_elevator_with_input = 20, ///< elevator, with rc input
k_rudder = 21, ///< secondary rudder channel
k_sprayer_pump = 22, ///< crop sprayer pump channel
k_sprayer_spinner = 23, ///< crop sprayer spinner channel
k_flaperon1 = 24, ///< flaperon, left wing
k_flaperon2 = 25, ///< flaperon, right wing
k_steering = 26, ///< ground steering, used to separate from rudder
k_parachute_release = 27, ///< parachute release
k_gripper = 28, ///< gripper
k_landing_gear_control = 29, ///< landing gear controller
k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters
k_heli_rsc = 31, ///< helicopter RSC output
k_heli_tail_rsc = 32, ///< helicopter tail RSC output
k_motor1 = 33, ///< these allow remapping of copter motors
k_motor2 = 34,
k_motor3 = 35,
k_motor4 = 36,
k_motor5 = 37,
k_motor6 = 38,
k_motor7 = 39,
k_motor8 = 40,
k_motor_tilt = 41, ///< tiltrotor motor tilt control
k_rcin1 = 51, ///< these are for pass-thru from arbitrary rc inputs
k_rcin2 = 52, //这里就是直接遥控控制到输出
k_rcin3 = 53,
k_rcin4 = 54,
k_rcin5 = 55,
k_rcin6 = 56,
k_rcin7 = 57,
k_rcin8 = 58,
k_rcin9 = 59,
k_rcin10 = 60,
k_rcin11 = 61,
k_rcin12 = 62,
k_rcin13 = 63,
k_rcin14 = 64,
k_rcin15 = 65,
k_rcin16 = 66,
k_ignition = 67,
k_choke = 68,
k_starter = 69,
k_throttle = 70,
k_tracker_yaw = 71, ///< antennatracker yaw
k_tracker_pitch = 72, ///< antennatracker pitch
k_throttleLeft = 73,
k_throttleRight = 74,
k_tiltMotorLeft = 75, ///< vectored thrust, left tilt
k_tiltMotorRight = 76, ///< vectored thrust, right tilt
k_elevon_left = 77,
k_elevon_right = 78,
k_vtail_left = 79,
k_vtail_right = 80,
k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one)
} Aux_servo_function_t;代码中对应的参数如下：
const AP_Param::GroupInfo AC_Sprayer::var_info[] = {
// @Param: ENABLE
// @DisplayName: Sprayer enable/disable
// @Description: Allows you to enable (1) or disable (0) the sprayer
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO_FLAGS("ENABLE", 0, AC_Sprayer, _enabled, 0, AP_PARAM_FLAG_ENABLE), //是否进行使能
// @Param: PUMP_RATE
// @DisplayName: Pump speed
// @Description: Desired pump speed when traveling 1m/s expressed as a percentage
// @Units: percentage
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("PUMP_RATE", 1, AC_Sprayer, _pump_pct_1ms, AC_SPRAYER_DEFAULT_PUMP_RATE), //期望的喷洒速度，这个参数设置大，比如100cm/s速度飞行，×10%，就是10%运行
// @Param: SPINNER
// @DisplayName: Spinner rotation speed
// @Description: Spinner's rotation speed in PWM (a higher rate will disperse the spray over a wider area horizontally)
// @Units: ms
// @Range: 1000 2000
// @User: Standard
AP_GROUPINFO("SPINNER", 2, AC_Sprayer, _spinner_pwm, AC_SPRAYER_DEFAULT_SPINNER_PWM), //水平喷洒旋转设置
// @Param: SPEED_MIN
// @DisplayName: Speed minimum
// @Description: Speed minimum at which we will begin spraying
// @Units: cm/s
// @Range: 0 1000
// @User: Standard
AP_GROUPINFO("SPEED_MIN", 3, AC_Sprayer, _speed_min, AC_SPRAYER_DEFAULT_SPEED_MIN), //大于这个值才能进行喷洒
// @Param: PUMP_MIN
// @DisplayName: Pump speed minimum
// @Description: Minimum pump speed expressed as a percentage
// @Units: percentage
// @Range: 0 100
// @User: Standard
AP_GROUPINFO("PUMP_MIN", 4, AC_Sprayer, _pump_min_pct, AC_SPRAYER_DEFAULT_PUMP_MIN), //默认的最小喷洒速度所占的百分比
AP_GROUPEND
};
（2）怎么使遥控器控制水泵
要想让水泵工作，需要遥控器的外部开关adc值的大小控制水泵的喷洒速度的大小；那么我们的水泵要连接具有pwm功能的引脚上面。这样我们就像控制无人机电机一样来控制水泵。（更形象的理解怎么控制电机转的过程）。
往下看这个代码
read_aux_switch(CH_7, aux_con.CH7_flag, g.ch7_option); //对应的是哪个通道的哪个值,这里设置控制水泵继续看代码
#define read_aux_switch(chan, flag, option) \
do { \
switch_position = read_3pos_switch(chan); \
if (flag != switch_position) { \
flag = switch_position; \
do_aux_switch_function(option, flag); \
} \
} while (false)
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
switch(ch_function)
{
case AUXSW_FLIP:
// flip if switch is on, positive throttle and we're actually flying
if (ch_flag == AUX_SWITCH_HIGH)
{
set_mode(FLIP, MODE_REASON_TX_COMMAND);
}
break;
case AUXSW_SIMPLE_MODE:
// low = simple mode off, middle or high position turns simple mode on
set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_SUPERSIMPLE_MODE:
// low = simple mode off, middle = simple mode, high = super simple mode
set_simple_mode(ch_flag);
break;
case AUXSW_RTL:
if (ch_flag == AUX_SWITCH_HIGH) {
// engage RTL (if not possible we remain in current flight mode)
set_mode(RTL, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in RTL
if (control_mode == RTL) {
reset_control_switch();
}
}
break;
case AUXSW_SAVE_TRIM:
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) {
save_trim();
}
break;
case AUXSW_SAVE_WP:
// save waypoint when switch is brought high
if (ch_flag == AUX_SWITCH_HIGH) {
// do not allow saving new waypoints while we're in auto or disarmed
if (control_mode == AUTO || !motors->armed()) {
return;
}
// do not allow saving the first waypoint with zero throttle
if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) {
return;
}
// create new mission command
AP_Mission::Mission_Command cmd = {};
// if the mission is empty save a takeoff command
if (mission.num_commands() == 0)
{
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF;
cmd.content.location.options = 0;
cmd.p1 = 0;
cmd.content.location.lat = 0;
cmd.content.location.lng = 0;
cmd.content.location.alt = MAX(current_loc.alt,100);
// use the current altitude for the target alt for takeoff.
// only altitude will matter to the AP mission script for takeoff.
if (mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
// set new waypoint to current location
cmd.content.location = current_loc;
// if throttle is above zero, create waypoint command
if (channel_throttle->get_control_in() > 0) {
cmd.id = MAV_CMD_NAV_WAYPOINT;
} else {
// with zero throttle, create LAND command
cmd.id = MAV_CMD_NAV_LAND;
}
// save command
if (mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
break;
case AUXSW_CAMERA_TRIGGER:
#if CAMERA == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
do_take_picture();
}
#endif
break;
case AUXSW_RANGEFINDER:
// enable or disable the rangefinder
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) {
rangefinder_state.enabled = true;
} else {
rangefinder_state.enabled = false;
}
#endif
break;
case AUXSW_FENCE:
#if AC_FENCE == ENABLED
// enable or disable the fence
if (ch_flag == AUX_SWITCH_HIGH) {
fence.enable(true);
Log_Write_Event(DATA_FENCE_ENABLE);
} else {
fence.enable(false);
Log_Write_Event(DATA_FENCE_DISABLE);
}
#endif
break;
case AUXSW_ACRO_TRAINER:
switch(ch_flag) {
case AUX_SWITCH_LOW:
g.acro_trainer = ACRO_TRAINER_DISABLED;
Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
g.acro_trainer = ACRO_TRAINER_LEVELING;
Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
break;
case AUX_SWITCH_HIGH:
g.acro_trainer = ACRO_TRAINER_LIMITED;
Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
break;
}
break;
case AUXSW_GRIPPER:
#if GRIPPER_ENABLED == ENABLED
switch(ch_flag) {
case AUX_SWITCH_LOW:
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case AUX_SWITCH_HIGH:
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
}
#endif
break;
case AUXSW_SPRAYER:
#if SPRAYER == ENABLED
sprayer.run(ch_flag == AUX_SWITCH_HIGH);
// if we are disarmed the pilot must want to test the pump
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors->armed());
#endif
break;
case AUXSW_AUTO:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(AUTO, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in AUTO
if (control_mode == AUTO) {
reset_control_switch();
}
}
break;
case AUXSW_AUTOTUNE:
#if AUTOTUNE_ENABLED == ENABLED
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
case AUX_SWITCH_MIDDLE:
// restore flight mode based on flight mode switch position
if (control_mode == AUTOTUNE) {
reset_control_switch();
}
break;
case AUX_SWITCH_HIGH:
// start an autotuning session
set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
break;
}
#endif
break;
case AUXSW_LAND:
if (ch_flag == AUX_SWITCH_HIGH)
{
set_mode(LAND, MODE_REASON_TX_COMMAND);
} else
{
// return to flight mode switch's flight mode if we are currently in LAND
if (control_mode == LAND) {
reset_control_switch();
}
}
break;
case AUXSW_PARACHUTE_ENABLE:
#if PARACHUTE == ENABLED
// Parachute enable/disable
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
#endif
break;
case AUXSW_PARACHUTE_RELEASE:
#if PARACHUTE == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
parachute_manual_release();
}
#endif
break;
case AUXSW_PARACHUTE_3POS:
#if PARACHUTE == ENABLED
// Parachute disable, enable, release with 3 position switch
switch (ch_flag) {
case AUX_SWITCH_LOW:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case AUX_SWITCH_HIGH:
parachute.enabled(true);
parachute_manual_release();
break;
}
#endif
break;
case AUXSW_MISSION_RESET:
if (ch_flag == AUX_SWITCH_HIGH) {
mission.reset();
}
break;
case AUXSW_ATTCON_FEEDFWD:
// enable or disable feed forward
attitude_control->bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_ATTCON_ACCEL_LIM:
// enable or disable accel limiting by restoring defaults
attitude_control->accel_limiting(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RETRACT_MOUNT:
#if MOUNT == ENABLE
switch (ch_flag) {
case AUX_SWITCH_HIGH:
camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
break;
case AUX_SWITCH_LOW:
camera_mount.set_mode_to_default();
break;
}
#endif
break;
case AUXSW_RELAY:
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY2:
ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY3:
ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY4:
ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_LANDING_GEAR:
switch (ch_flag) {
case AUX_SWITCH_LOW:
landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
break;
case AUX_SWITCH_HIGH:
landinggear.set_position(AP_LandingGear::LandingGear_Retract);
break;
}
break;
case AUXSW_LOST_COPTER_SOUND:
switch (ch_flag) {
case AUX_SWITCH_HIGH:
AP_Notify::flags.vehicle_lost = true;
break;
case AUX_SWITCH_LOW:
AP_Notify::flags.vehicle_lost = false;
break;
}
break;
case AUXSW_MOTOR_ESTOP:
// Turn on Emergency Stop logic when channel is high
set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_MOTOR_INTERLOCK:
// Turn on when above LOW, because channel will also be used for speed
// control signal in tradheli
ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_BRAKE:
// brake flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(BRAKE, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in BRAKE
if (control_mode == BRAKE) {
reset_control_switch();
}
}
break;
case AUXSW_THROW:
// throw flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(THROW, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in throw mode
if (control_mode == THROW) {
reset_control_switch();
}
}
break;
case AUXSW_AVOID_ADSB:
// enable or disable AP_Avoidance
if (ch_flag == AUX_SWITCH_HIGH) {
avoidance_adsb.enable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
} else {
avoidance_adsb.disable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
break;
case AUXSW_PRECISION_LOITER:
#if PRECISION_LANDING == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
set_precision_loiter_enabled(true);
break;
case AUX_SWITCH_LOW:
set_precision_loiter_enabled(false);
break;
}
#endif
break;
case AUXSW_AVOID_PROXIMITY:
#if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
avoid.proximity_avoidance_enable(true);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE);
break;
case AUX_SWITCH_LOW:
avoid.proximity_avoidance_enable(false);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE);
break;
}
#endif
break;
case AUXSW_ARMDISARM:
// arm or disarm the vehicle
switch (ch_flag) {
case AUX_SWITCH_HIGH:
init_arm_motors(false);
break;
case AUX_SWITCH_LOW:
init_disarm_motors();
break;
}
break;
}
}我们重点看这个代码
下面我们重点分析这个代码
void test_pump(bool true_false) { _flags.testing = true_false; }//这个是测试代码总结：到这里初始化的讲解基本设置好，主要设置通道，使能喷洒，还有参数设置，下面讲解代码运行
2.喷洒作业更新运行
SCHED_TASK(three_hz_loop, 3, 75), //植保喷洒设置
void AC_Sprayer::update()
{
//如果我们被禁用立即返回-------exit immediately if we are disabled or shouldn't be running
if (!_enabled || !running()) //_enabled=0|| _flags.running=0,直接返回
{
run(false);//直接停止，返回，不进行植保作业喷洒
return;
}
//如果没有为任何伺服机构设置泵功能，立即退出---- exit immediately if the pump function has not been set-up for any servo
if (!SRV_Channels::function_assigned(SRV_Channel::k_sprayer_pump))
{
return;
}
//获取水平速度信息-----------------------------------get horizontal velocity
const Vector3f &velocity = _inav->get_velocity();
float ground_speed = norm(velocity.x,velocity.y);
//获取当前时间--------------------------------------get the current time
const uint32_t now = AP_HAL::millis();
bool should_be_spraying = _flags.spraying;
//检测当前的速度是否最小------------------------------check our speed vs the minimum
if (ground_speed >= _speed_min)
{
//如果我们还没有喷洒-----------------------------if we are not already spraying
if (!_flags.spraying)
{
//设置定时器，如果这是我们第一次超过最小速度---- set the timer if this is the first time we've surpassed the min speed
if (_speed_over_min_time == 0)
{
_speed_over_min_time = now;
}else
{
//检查我们是否已经超过了足够长的速度来接合喷雾器-- check if we've been over the speed long enough to engage the sprayer
if((now - _speed_over_min_time) > AC_SPRAYER_DEFAULT_TURN_ON_DELAY)
{
should_be_spraying = true;
_speed_over_min_time = 0;
}
}
}
//复位更新时间---------reset the speed under timer
_speed_under_min_time = 0;
}else
{
// 在最小速度以下---------------------we are under the min speed.
if (_flags.spraying)
{
//设置定时器，如果这是我们第一次降到最小速度以下--- set the timer if this is the first time we've dropped below the min speed
if (_speed_under_min_time == 0)
{
_speed_under_min_time = now;
}else
{
//检查我们是否已经超过了足够长的速度来接合喷雾器------check if we've been over the speed long enough to engage the sprayer
if((now - _speed_under_min_time) > AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY)
{
should_be_spraying = false;
_speed_under_min_time = 0;
}
}
}
// reset the speed over timer
_speed_over_min_time = 0;
}
// if testing pump output speed as if traveling at 1m/s
if (_flags.testing)
{
ground_speed = 100.0f;
should_be_spraying = true;
}
//如果喷洒装置或测试更新泵伺服位置----------if spraying or testing update the pump servo position
if (should_be_spraying) //可以喷洒了
{
float pos = ground_speed * _pump_pct_1ms;
pos = MAX(pos, 100 *_pump_min_pct); // ensure min pump speed
pos = MIN(pos,10000); // clamp to range
SRV_Channels::move_servo(SRV_Channel::k_sprayer_pump, pos, 0, 10000);
SRV_Channels::set_output_pwm(SRV_Channel::k_sprayer_spinner, _spinner_pwm);
_flags.spraying = true;
}else
{
stop_spraying();
}
}