ArduPilot开源飞控之AP_Relay

1. 源由

开关动作基本上都是Relay来完成的。

从工作原理上来说，就是控制GPIO的高低电平，触发后续机械动作，比如：伺服开关/相机拍照等。

2. 框架设计

2.1 启动代码

Copter::init_ardupilot
 └──> AP_Relay::init
1
2

2.2 任务代码

2.2.1 AP_Camera_Relay::update

SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111)
 └──> AP_Camera::update
     └──> AP_Camera_Relay::update
1
2
3

2.2.2 AP_Parachute::update

FAST_TASK(update_land_and_crash_detectors),
 └──> Copter::update_land_and_crash_detectors
     └──> Copter::parachute_check
         └──> AP_Parachute::update
1
2
3
4

2.2.3 MAVLink命令

• AP_Parachute::arming_checks
• AP_Camera_Relay::trigger_pic
• AP_ServoRelayEvents::update_events

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102)
 └──> GCS::update_receive
     └──> GCS_MAVLINK::update_receive
         └──> GCS_MAVLINK::packetReceived
             └──> GCS_MAVLINK_Copter::handleMessage
                 └──> GCS_MAVLINK::handle_common_message
                     ├──> GCS_MAVLINK::handle_command_long
                     │   └──> GCS_MAVLINK_Copter::handle_command_long_packet
                     │       └──> GCS_MAVLINK::handle_command_long_packet
                     │           ├──> GCS_MAVLINK::handle_command_run_prearm_checks
                     │           │   └──> AP_Arming_Copter::pre_arm_checks
                     │           │       └──> AP_Arming::pre_arm_checks
                     │           │           └──> AP_Arming::system_checks
                     │           │               └──> AP_Parachute::arming_checks
                     │           └──> GCS_MAVLINK::handle_command_camera
                     │               └──> AP_Camera::handle_command_long
                     │                   └──> AP_Camera::take_picture
                     │                       └──> AP_Camera_Backend::take_picture
                     │                           └──> AP_Camera_Relay::trigger_pic
                     └──> GCS_MAVLINK::handle_command_int
                         └──> GCS_MAVLINK::handle_command_int_packet
                             └──> GCS_MAVLINK::handle_servorelay_message
                                 ├──> AP_ServoRelayEvents::do_repeat_servo
                                 │   └──> AP_ServoRelayEvents::update_events
                                 └──> AP_ServoRelayEvents::do_repeat_relay
                                     └──> AP_ServoRelayEvents::update_events
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

3. 重要例程

3.1 AP_Relay::AP_Relay

SITL和Linux板子有特殊默认引脚，其他板子默认没有pin脚定义。

AP_Relay::AP_Relay(void)
 ├──> AP_Param::setup_object_defaults(this, var_info)
 ├──>  
 │   └──> AP_HAL::panic("AP_Relay must be singleton")
 └──> singleton = this
1
2
3
4
5

3.2 AP_Relay::init

默认启动Relay IO配置为0输出，可通过RELAY_DEFAULT配置。

AP_Relay::init
 ├──> <_default != 0 && _default != 1>
 │   └──> return
 └──> 
     └──> set(i, _default)
1
2
3
4
5

3.3 AP_Camera_Relay::update

// update - should be called at 50hz
AP_Camera_Relay::update
 │
 │  /********************************************************************************
 │   * Trigger delay                                                                *
 │   ********************************************************************************/
 ├──>  0>
 │   └──> trigger_counter--
 │
 │  /********************************************************************************
 │   * On/Off                                                                       *
 │   ********************************************************************************/
 ├──> < else >
 │   ├──> AP_Relay *ap_relay = AP::relay()
 │   ├──> 
 │   │   └──> return
 │   ├──> <_params.relay_on>
 │   │   └──> ap_relay->off(0)
 │   └──> < else >
 │       └──> ap_relay->on(0)
 │
 │  /********************************************************************************
 │   * call parent update                                                           *
 │   ********************************************************************************/
 └──> AP_Camera_Backend::update()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

3.4 AP_Parachute::update

降落伞状态控制。

/// update - shuts off the trigger should be called at about 10hz
AP_Parachute::update
 │
 │  /********************************************************************************
 │   * exit immediately if not enabled or parachute not to be released              *
 │   ********************************************************************************/
 ├──> <_enabled <= 0>
 │   └──> return
 │
 │   // calc time since release
 ├──> uint32_t time_diff = AP_HAL::millis() - _release_time
 ├──> uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms
 │
 ├──> bool hold_forever = (_options.get() & uint32_t(Options::HoldOpen)) != 0
 │
 │   // check if we should release parachute
 ├──> <(_release_time != 0) && !_release_in_progress>
 │   │/********************************************************************************
 │   │ * release parachute                                                            *
 │   │ ********************************************************************************/
 │   ├──> = delay_ms>
 │   │   ├──> <_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO> 
 │   │   │   │   // PWM output
 │   │   │   └──> SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_on_pwm)  // move servo
 │   │   └──> < else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3)> 
 │   │       │   // High voltage output
 │   │       ├──> AP_Relay*_relay = AP::relay() // set relay
 │   │       └──> <_relay != nullptr> _relay->on(_release_type)
 │   ├──> _release_in_progress = true
 │   └──> _released = true
 └──> < else if ((_release_time == 0) || (!hold_forever && time_diff >= delay_ms + AP_PARACHUTE_RELEASE_DURATION_MS))>
     │/********************************************************************************
     │ * release parachute end                                                        *
     │ ********************************************************************************/
     ├──> <_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO>
	 │   │   // PWM output
     │   └──> SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_off_pwm) // move servo back to off position
     ├──> < else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3)> 
	 │   │   // Low voltage output
     │   ├──> AP_Relay*_relay = AP::relay()  // set relay back to zero volts
     │   └──> <_relay != nullptr> _relay->off(_release_type)
     │
     │   // reset released flag and release_time
     ├──> _release_in_progress = false
     ├──> _release_time = 0
     │
     │   // update AP_Notify
     └──> AP_Notify::flags.parachute_release = 0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

3.5 AP_Parachute::arming_checks

解锁降落山配置检查。

// check settings are valid
AP_Parachute::arming_checks
 ├──> <_enabled > 0>
 │   │/********************************************************************************
 │   │ * Parachute Configuration Check                                                *
 │   │ ********************************************************************************/
 │   ├──> <_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
 │   │   └──> 
 │   │       ├──> hal.util->snprintf(buffer, buflen, "Chute has no channel")
 │   │       └──> return false
 │   ├──> < else >
 │   │   ├──> 
 │   │   │   ├──> AP_Relay*_relay = AP::relay()
 │   │   │   └──> <_relay == nullptr || !_relay->enabled(_release_type)>
 │   │   │       ├──> hal.util->snprintf(buffer, buflen, "Chute invalid relay %d", int(_release_type))
 │   │   │       └──> return false
 │   │   └──> 
 │   │       └──> hal.util->snprintf(buffer, buflen, "AP_Relay not available")
 │   └──> <_release_initiated>
 │       ├──> hal.util->snprintf(buffer, buflen, "Chute is released")
 │       └──> return false
 │
 │  /********************************************************************************
 │   * No Parachute Configuration                                                   *
 │   ********************************************************************************/
 └──> return true
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

3.6 AP_Camera_Relay::trigger_pic

拍照控制。

// entry point to actually take a picture.  returns true on success
AP_Camera_Relay::trigger_pic
 │   // fail if have not completed previous picture
 ├──>  0>
 │   └──> return false
 │
 │  /********************************************************************************
 │   * On/Off                                                                       *
 │   ********************************************************************************/
 │   // exit immediately if no relay is setup
 ├──> AP_Relay *ap_relay = AP::relay()
 ├──> 
 │   └──> return false
 ├──> <_params.relay_on>
 │   └──> ap_relay->on(0)
 ├──> < else >
 │   └──> ap_relay->off(0)
 │
 │  /********************************************************************************
 │   * set counter to move servo to off position                                    *
 │   * after this many iterations of update (assumes 50hz update rate)              *
 │   ********************************************************************************/
 ├──> trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX)
 └──> return true
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

3.7 AP_ServoRelayEvents::update_events

/*
  update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
*/
AP_ServoRelayEvents::update_events
 │
 │  /********************************************************************************
 │   * repeat condition check                                                       *
 │   ********************************************************************************/
 ├──> 
 │   └──> return
 │
 ├──> start_time_ms = AP_HAL::millis()
 │
 │  /********************************************************************************
 │   * EVENT_TYPE_SERVO                                                             *
 │   ********************************************************************************/
 ├──> 
 │   ├──> SRV_Channel *c = SRV_Channels::srv_channel(channel-1)
 │   └──> 
 │       ├──> 
 │       │   └──> c->set_output_pwm(c->get_trim())
 │       └──> < else >
 │           ├──> c->set_output_pwm(servo_value)
 │           └──> c->ignore_small_rcin_changes()
 │
 │  /********************************************************************************
 │   * EVENT_TYPE_RELAY                                                             *
 │   ********************************************************************************/
 ├──>  
 │   ├──> AP_Relay *relay = AP::relay()
 │   └──> 
 │       └──> relay->toggle(channel)
 │
 │  /********************************************************************************
 │   * set counter to move servo to off position                                    *
 │   ********************************************************************************/
 ├──>  0>
 │   └──> repeat--
 └──> < else > // toggle bottom bit so servos flip in value
     └──> repeat ^= 1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

4. 总结

Relay目前有以下几个应用场景：

1. AP_Parachute
2. AP_Camera_Relay
3. AP_ServoRelayEvents

配置参数：

• RELAY_ Parameters
• BTN_ Parameters
• ADSB_ Parameters
• CHUTE_ Parameters
• RC Parameters
• SERVO Parameters

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计