mirror of https://github.com/ArduPilot/ardupilot
104 lines
3.1 KiB
C++
104 lines
3.1 KiB
C++
|
/*
|
||
|
|
* This file 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 file 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/>.
|
||
|
|
*
|
||
|
|
* Code by Andy Piper and Siddharth Bharat Purohit
|
||
|
|
*
|
||
|
|
* There really is no dshot reference. For information try these resources:
|
||
|
|
* https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/
|
||
|
|
* https://www.swallenhardware.io/battlebots/2019/4/20/a-developers-guide-to-dshot-escs
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <hal.h>
|
||
|
|
|
||
|
|
#if defined(IOMCU_FW) && HAL_DSHOT_ENABLED
|
||
|
|
// need to give the little guy as much help as possible
|
||
|
|
#pragma GCC optimize("O2")
|
||
|
|
|
||
|
|
#include "RCOutput.h"
|
||
|
|
#include <AP_Math/AP_Math.h>
|
||
|
|
#include "GPIO.h"
|
||
|
|
#include "Scheduler.h"
|
||
|
|
|
||
|
|
#if HAL_USE_PWM == TRUE
|
||
|
|
|
||
|
|
using namespace ChibiOS;
|
||
|
|
|
||
|
|
extern const AP_HAL::HAL& hal;
|
||
|
|
|
||
|
|
THD_WORKING_AREA(dshot_thread_wa, 64);
|
||
|
|
void RCOutput::timer_tick()
|
||
|
|
{
|
||
|
|
if (dshot_timer_setup) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool dshot_enabled = false;
|
||
|
|
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
||
|
|
pwm_group &group = pwm_group_list[i];
|
||
|
|
if (is_dshot_protocol(group.current_mode)) {
|
||
|
|
dshot_enabled = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (!dshot_timer_setup && dshot_enabled) {
|
||
|
|
chThdCreateStatic(dshot_thread_wa, sizeof(dshot_thread_wa),
|
||
|
|
APM_RCOUT_PRIORITY, &RCOutput::dshot_send_trampoline, this);
|
||
|
|
dshot_timer_setup = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void RCOutput::dshot_send_trampoline(void *p)
|
||
|
|
{
|
||
|
|
RCOutput *rcout = (RCOutput *)p;
|
||
|
|
rcout->rcout_thread();
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
thread for handling RCOutput send on IOMCU
|
||
|
|
*/
|
||
|
|
void RCOutput::rcout_thread() {
|
||
|
|
// don't start outputting until fully configured
|
||
|
|
while (!hal.scheduler->is_system_initialized()) {
|
||
|
|
hal.scheduler->delay_microseconds(1000);
|
||
|
|
}
|
||
|
|
|
||
|
|
rcout_thread_ctx = chThdGetSelfX();
|
||
|
|
|
||
|
|
while (true) {
|
||
|
|
chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
|
||
|
|
|
||
|
|
// this is when the cycle is supposed to start
|
||
|
|
if (_dshot_cycle == 0) {
|
||
|
|
// register a timer for the next tick if push() will not be providing it
|
||
|
|
if (_dshot_rate != 1) {
|
||
|
|
chVTSet(&_dshot_rate_timer, chTimeUS2I(_dshot_period_us), dshot_update_tick, this);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// main thread requested a new dshot send or we timed out - if we are not running
|
||
|
|
// as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity
|
||
|
|
dshot_send_groups(0);
|
||
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
||
|
|
dshot_collect_dma_locks(0);
|
||
|
|
#endif
|
||
|
|
if (_dshot_rate > 0) {
|
||
|
|
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif // HAL_USE_PWM
|
||
|
|
|
||
|
|
#endif // IOMCU_FW && HAL_DSHOT_ENABLED
|