1. 源由
开关动作基本上都是Relay来完成的。
从工作原理上来说,就是控制GPIO的高低电平,触发后续机械动作,比如:伺服开关/相机拍照等。
2. 框架设计
2.1 启动代码
Copter::init_ardupilot
└──> AP_Relay::init
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
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
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
3. 重要例程
3.1 AP_Relay::AP_Relay
SITL和Linux板子有特殊默认引脚,其他板子默认没有pin脚定义。
AP_Relay::AP_Relay(void)
├──> AP_Param::setup_object_defaults(this, var_info)
├──> <CONFIG_HAL_BOARD == HAL_BOARD_SITL> <singleton != nullptr>
│ └──> AP_HAL::panic("AP_Relay must be singleton")
└──> singleton = this
3.2 AP_Relay::init
默认启动Relay IO配置为0输出,可通过RELAY_DEFAULT
配置。
Value | Meaning |
---|---|
0 | off |
1 | on |
2 | no change |
AP_Relay::init
├──> <_default != 0 && _default != 1>
│ └──> return
└──> <for (uint8_t i=0 i<AP_RELAY_NUM_RELAYS i++)>
└──> set(i, _default)
3.3 AP_Camera_Relay::update
// update - should be called at 50hz
AP_Camera_Relay::update
│
│ /********************************************************************************
│ * Trigger delay *
│ ********************************************************************************/
├──> <trigger_counter > 0>
│ └──> trigger_counter--
│
│ /********************************************************************************
│ * On/Off *
│ ********************************************************************************/
├──> < else >
│ ├──> AP_Relay *ap_relay = AP::relay()
│ ├──> <ap_relay == nullptr>
│ │ └──> return
│ ├──> <_params.relay_on>
│ │ └──> ap_relay->off(0)
│ └──> < else >
│ └──> ap_relay->on(0)
│
│ /********************************************************************************
│ * call parent update *
│ ********************************************************************************/
└──> AP_Camera_Backend::update()
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 *
│ │ ********************************************************************************/
│ ├──> <time_diff >= 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)> <AP_RELAY_ENABLED>
│ │ │ // 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)> <AP_RELAY_ENABLED>
│ │ // 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
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) {
│ │ └──> <!SRV_Channels::function_assigned(SRV_Channel::k_parachute_release)>
│ │ ├──> hal.util->snprintf(buffer, buflen, "Chute has no channel")
│ │ └──> return false
│ ├──> < else >
│ │ ├──> <AP_RELAY_ENABLED>
│ │ │ ├──> 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
│ │ └──> <else>
│ │ └──> 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
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
├──> <trigger_counter > 0>
│ └──> return false
│
│ /********************************************************************************
│ * On/Off *
│ ********************************************************************************/
│ // exit immediately if no relay is setup
├──> AP_Relay *ap_relay = AP::relay()
├──> <ap_relay == nullptr>
│ └──> 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
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 *
│ ********************************************************************************/
├──> <repeat == 0 || (AP_HAL::millis() - start_time_ms) < delay_ms>
│ └──> return
│
├──> start_time_ms = AP_HAL::millis()
│
│ /********************************************************************************
│ * EVENT_TYPE_SERVO *
│ ********************************************************************************/
├──> <case EVENT_TYPE_SERVO>
│ ├──> SRV_Channel *c = SRV_Channels::srv_channel(channel-1)
│ └──> <c != nullptr>
│ ├──> <repeat & 1>
│ │ └──> c->set_output_pwm(c->get_trim())
│ └──> < else >
│ ├──> c->set_output_pwm(servo_value)
│ └──> c->ignore_small_rcin_changes()
│
│ /********************************************************************************
│ * EVENT_TYPE_RELAY *
│ ********************************************************************************/
├──> <case EVENT_TYPE_RELAY> <AP_RELAY_ENABLED>
│ ├──> AP_Relay *relay = AP::relay()
│ └──> <relay != nullptr>
│ └──> relay->toggle(channel)
│
│ /********************************************************************************
│ * set counter to move servo to off position *
│ ********************************************************************************/
├──> <repeat > 0>
│ └──> repeat--
└──> < else > // toggle bottom bit so servos flip in value
└──> repeat ^= 1
4. 总结
Relay目前有以下几个应用场景:
- AP_Parachute
- AP_Camera_Relay
- AP_ServoRelayEvents
配置参数:
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计