mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_HAL_ChibiOS: add support for bidir DShot support in RCOutput
add support for sampling GPIO pins using timer don't restart pwm group when not doing bi-dir fix hwdef generation preproc for TIM DMA decode telemetry at the start of the dshot cycle calculate dshot pulse separation correctly and ensure we output rapidly enough calculate dshot min periods and timeouts correctly refactor dshot_send() into dshot_send_groups() use bi-dir dshot channel mask selectively enable bi-dir RC Channels process bi-dir mask correctly when allocating DMA channels allow UP and CH DMA channel sharing optionally enable bidir vars in hwdef. enable bi-dir dshot in KakuteF7Mini enable bi-dir dshot in OmnibusF4Pro enable bi-dir dshot in OmnibusNanoV6 enable bi-dir dshot in MatekF405 enable bi-dir dshot in fmuv5 enable bi-dir dshot in fmuv3 enable bi-dir dshot in OmnibusF7V2 enable bi-dir dshot in OmnibusNanoV6 enable bi-dir dshot in CubeOrange enable bi-dir dshot in Pixracer enable bi-dir dshot in mRoPixracerPro Co-authored-by: bugobliterator <siddharthbharatpurohit@gmail.com>
This commit is contained in:
parent
b5688c023c
commit
401e5c2073
@ -13,6 +13,7 @@
|
|||||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*
|
*
|
||||||
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
||||||
|
* Bi-directional dshot based on Betaflight, code by Andy Piper and Siddharth Bharat Purohit
|
||||||
*/
|
*/
|
||||||
#include "RCOutput.h"
|
#include "RCOutput.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
@ -21,6 +22,8 @@
|
|||||||
#include "GPIO.h"
|
#include "GPIO.h"
|
||||||
#include "hwdef/common/stm32_util.h"
|
#include "hwdef/common/stm32_util.h"
|
||||||
#include "hwdef/common/watchdog.h"
|
#include "hwdef/common/watchdog.h"
|
||||||
|
#include <AP_InternalError/AP_InternalError.h>
|
||||||
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
|
|
||||||
#if HAL_USE_PWM == TRUE
|
#if HAL_USE_PWM == TRUE
|
||||||
|
|
||||||
@ -35,10 +38,17 @@ extern AP_IOMCU iomcu;
|
|||||||
|
|
||||||
#define RCOU_SERIAL_TIMING_DEBUG 0
|
#define RCOU_SERIAL_TIMING_DEBUG 0
|
||||||
|
|
||||||
|
#if RCOU_DSHOT_TIMING_DEBUG
|
||||||
|
#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
|
||||||
|
#else
|
||||||
|
#define TOGGLE_PIN_DEBUG(pin) do {} while (0)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define TELEM_IC_SAMPLE 16
|
||||||
|
|
||||||
struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
|
struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
|
||||||
struct RCOutput::irq_state RCOutput::irq;
|
struct RCOutput::irq_state RCOutput::irq;
|
||||||
|
const uint8_t RCOutput::NUM_GROUPS = ARRAY_SIZE(RCOutput::pwm_group_list);
|
||||||
#define NUM_GROUPS ARRAY_SIZE(pwm_group_list)
|
|
||||||
|
|
||||||
// marker for a disabled channel
|
// marker for a disabled channel
|
||||||
#define CHAN_DISABLED 255
|
#define CHAN_DISABLED 255
|
||||||
@ -64,6 +74,9 @@ void RCOutput::init()
|
|||||||
num_fmu_channels = MAX(num_fmu_channels, group.chan[j]+1);
|
num_fmu_channels = MAX(num_fmu_channels, group.chan[j]+1);
|
||||||
group.ch_mask |= (1U<<group.chan[j]);
|
group.ch_mask |= (1U<<group.chan[j]);
|
||||||
}
|
}
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
group.bdshot.telem_tim_ch[j] = CHAN_DISABLED;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
if (group.ch_mask != 0) {
|
if (group.ch_mask != 0) {
|
||||||
pwmStart(group.pwm_drv, &group.pwm_cfg);
|
pwmStart(group.pwm_drv, &group.pwm_cfg);
|
||||||
@ -87,6 +100,13 @@ void RCOutput::init()
|
|||||||
#ifdef HAL_GPIO_PIN_SAFETY_IN
|
#ifdef HAL_GPIO_PIN_SAFETY_IN
|
||||||
safety_state = AP_HAL::Util::SAFETY_DISARMED;
|
safety_state = AP_HAL::Util::SAFETY_DISARMED;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if RCOU_DSHOT_TIMING_DEBUG
|
||||||
|
hal.gpio->pinMode(54, 1);
|
||||||
|
hal.gpio->pinMode(55, 1);
|
||||||
|
hal.gpio->pinMode(56, 1);
|
||||||
|
hal.gpio->pinMode(57, 1);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -401,7 +421,7 @@ void RCOutput::push_local(void)
|
|||||||
#ifndef DISABLE_DSHOT
|
#ifndef DISABLE_DSHOT
|
||||||
else if (is_dshot_protocol(group.current_mode) || group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED) {
|
else if (is_dshot_protocol(group.current_mode) || group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED) {
|
||||||
// set period_us to time for pulse output, to enable very fast rates
|
// set period_us to time for pulse output, to enable very fast rates
|
||||||
period_us = dshot_pulse_time_us;
|
period_us = group.dshot_pulse_time_us;
|
||||||
}
|
}
|
||||||
#endif //#ifndef DISABLE_DSHOT
|
#endif //#ifndef DISABLE_DSHOT
|
||||||
if (period_us > widest_pulse) {
|
if (period_us > widest_pulse) {
|
||||||
@ -525,7 +545,15 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
|
|||||||
|
|
||||||
// hold the lock during setup, to ensure there isn't a DMA operation ongoing
|
// hold the lock during setup, to ensure there isn't a DMA operation ongoing
|
||||||
group.dma_handle->lock();
|
group.dma_handle->lock();
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
// configure input capture DMA if required
|
||||||
|
if (is_bidir_dshot_enabled()) {
|
||||||
|
if (!bdshot_setup_group_ic_DMA(group)) {
|
||||||
|
group.dma_handle->unlock();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
// configure timer driver for DMAR at requested rate
|
// configure timer driver for DMAR at requested rate
|
||||||
if (group.pwm_started) {
|
if (group.pwm_started) {
|
||||||
pwmStop(group.pwm_drv);
|
pwmStop(group.pwm_drv);
|
||||||
@ -572,9 +600,9 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
|
|||||||
pwmmode_t mode = group.pwm_cfg.channels[j].mode;
|
pwmmode_t mode = group.pwm_cfg.channels[j].mode;
|
||||||
if (mode != PWM_OUTPUT_DISABLED) {
|
if (mode != PWM_OUTPUT_DISABLED) {
|
||||||
if(mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW || mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH) {
|
if(mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW || mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH) {
|
||||||
group.pwm_cfg.channels[j].mode = active_high?PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH:PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW;
|
group.pwm_cfg.channels[j].mode = active_high ? PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH : PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW;
|
||||||
} else {
|
} else {
|
||||||
group.pwm_cfg.channels[j].mode = active_high?PWM_OUTPUT_ACTIVE_HIGH:PWM_OUTPUT_ACTIVE_LOW;
|
group.pwm_cfg.channels[j].mode = active_high ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_ACTIVE_LOW;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -587,6 +615,7 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
|
|||||||
pwmEnableChannel(group.pwm_drv, j, 0);
|
pwmEnableChannel(group.pwm_drv, j, 0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
group.dma_handle->unlock();
|
group.dma_handle->unlock();
|
||||||
return true;
|
return true;
|
||||||
#else
|
#else
|
||||||
@ -594,7 +623,6 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
|
|||||||
#endif //#ifndef DISABLE_DSHOT
|
#endif //#ifndef DISABLE_DSHOT
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup output mode for a group, using group.current_mode. Used to restore output
|
setup output mode for a group, using group.current_mode. Used to restore output
|
||||||
after serial operations
|
after serial operations
|
||||||
@ -606,6 +634,8 @@ void RCOutput::set_group_mode(pwm_group &group)
|
|||||||
group.pwm_started = false;
|
group.pwm_started = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
memset(group.bdshot.erpm, 0, 4*sizeof(uint16_t));
|
||||||
|
|
||||||
switch (group.current_mode) {
|
switch (group.current_mode) {
|
||||||
case MODE_PWM_BRUSHED:
|
case MODE_PWM_BRUSHED:
|
||||||
// force zero output initially
|
// force zero output initially
|
||||||
@ -644,22 +674,29 @@ void RCOutput::set_group_mode(pwm_group &group)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// calculate min time between pulses
|
// calculate min time between pulses
|
||||||
dshot_pulse_time_us = 1000000UL * bit_length / rate;
|
group.dshot_pulse_time_us = 1000000UL * bit_length / rate;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: {
|
case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: {
|
||||||
const uint32_t rate = protocol_bitrate(group.current_mode);
|
const uint32_t rate = protocol_bitrate(group.current_mode);
|
||||||
const uint32_t bit_period = 20;
|
const uint32_t bit_period = 20;
|
||||||
|
bool active_high = is_bidir_dshot_enabled() ? false : true;
|
||||||
|
|
||||||
// configure timer driver for DMAR at requested rate
|
// configure timer driver for DMAR at requested rate
|
||||||
if (!setup_group_DMA(group, rate, bit_period, true, dshot_buffer_length, false)) {
|
if (!setup_group_DMA(group, rate, bit_period, active_high,
|
||||||
|
MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), false)) {
|
||||||
group.current_mode = MODE_PWM_NORMAL;
|
group.current_mode = MODE_PWM_NORMAL;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate min time between pulses
|
// calculate min time between pulses
|
||||||
dshot_pulse_time_us = 1000000UL * dshot_bit_length / rate;
|
group.dshot_pulse_send_time_us = 1000000UL * dshot_bit_length / rate;
|
||||||
|
if (is_bidir_dshot_enabled()) {
|
||||||
|
// to all intents and purposes the pulse time of send and receive are the same
|
||||||
|
group.dshot_pulse_time_us = group.dshot_pulse_send_time_us + group.dshot_pulse_send_time_us + 30;
|
||||||
|
} else {
|
||||||
|
group.dshot_pulse_time_us = group.dshot_pulse_send_time_us;
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -872,12 +909,7 @@ void RCOutput::trigger_groups(void)
|
|||||||
osalSysUnlock();
|
osalSysUnlock();
|
||||||
|
|
||||||
if (!serial_group) {
|
if (!serial_group) {
|
||||||
for (uint8_t i = 0; i < NUM_GROUPS; i++) {
|
dshot_send_groups(false);
|
||||||
pwm_group &group = pwm_group_list[i];
|
|
||||||
if (is_dshot_protocol(group.current_mode)) {
|
|
||||||
dshot_send(group, false);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -891,25 +923,22 @@ void RCOutput::trigger_groups(void)
|
|||||||
|
|
||||||
/*
|
/*
|
||||||
periodic timer. This is used for oneshot and dshot modes, plus for
|
periodic timer. This is used for oneshot and dshot modes, plus for
|
||||||
safety switch update
|
safety switch update. Runs every 1000us.
|
||||||
*/
|
*/
|
||||||
void RCOutput::timer_tick(void)
|
void RCOutput::timer_tick(void)
|
||||||
{
|
{
|
||||||
safety_update();
|
safety_update();
|
||||||
|
|
||||||
uint64_t now = AP_HAL::micros64();
|
uint64_t now = AP_HAL::micros64();
|
||||||
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
|
||||||
pwm_group &group = pwm_group_list[i];
|
// do a blocking send now, to guarantee DShot sends at
|
||||||
if (!serial_group &&
|
// above 1000 Hz. This makes the protocol more reliable on
|
||||||
is_dshot_protocol(group.current_mode) &&
|
// long cables, and also keeps some ESCs happy that don't
|
||||||
now - group.last_dmar_send_us > 400) {
|
// like low rates
|
||||||
// do a blocking send now, to guarantee DShot sends at
|
if (!serial_group) {
|
||||||
// above 1000 Hz. This makes the protocol more reliable on
|
dshot_send_groups(true);
|
||||||
// long cables, and also keeps some ESCs happy that don't
|
|
||||||
// like low rates
|
|
||||||
dshot_send(group, true);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (serial_led_pending && chMtxTryLock(&trigger_mutex)) {
|
if (serial_led_pending && chMtxTryLock(&trigger_mutex)) {
|
||||||
serial_led_pending = false;
|
serial_led_pending = false;
|
||||||
for (uint8_t j = 0; j < NUM_GROUPS; j++) {
|
for (uint8_t j = 0; j < NUM_GROUPS; j++) {
|
||||||
@ -942,7 +971,7 @@ void RCOutput::dma_allocate(Shared_DMA *ctx)
|
|||||||
pwm_group &group = pwm_group_list[i];
|
pwm_group &group = pwm_group_list[i];
|
||||||
if (group.dma_handle == ctx && group.dma == nullptr) {
|
if (group.dma_handle == ctx && group.dma == nullptr) {
|
||||||
chSysLock();
|
chSysLock();
|
||||||
group.dma = dmaStreamAllocI(group.dma_up_stream_id, 10, dma_irq_callback, &group);
|
group.dma = dmaStreamAllocI(group.dma_up_stream_id, 10, dma_up_irq_callback, &group);
|
||||||
chSysUnlock();
|
chSysUnlock();
|
||||||
#if STM32_DMA_SUPPORTS_DMAMUX
|
#if STM32_DMA_SUPPORTS_DMAMUX
|
||||||
if (group.dma) {
|
if (group.dma) {
|
||||||
@ -972,7 +1001,7 @@ void RCOutput::dma_deallocate(Shared_DMA *ctx)
|
|||||||
/*
|
/*
|
||||||
create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight
|
create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight
|
||||||
*/
|
*/
|
||||||
uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request)
|
uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem)
|
||||||
{
|
{
|
||||||
uint16_t packet = (value << 1);
|
uint16_t packet = (value << 1);
|
||||||
|
|
||||||
@ -987,8 +1016,13 @@ uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request)
|
|||||||
csum ^= csum_data;
|
csum ^= csum_data;
|
||||||
csum_data >>= 4;
|
csum_data >>= 4;
|
||||||
}
|
}
|
||||||
csum &= 0xf;
|
// trigger bi-dir dshot telemetry
|
||||||
|
if (bidir_telem) {
|
||||||
|
csum = ~csum;
|
||||||
|
}
|
||||||
|
|
||||||
// append checksum
|
// append checksum
|
||||||
|
csum &= 0xf;
|
||||||
packet = (packet << 4) | csum;
|
packet = (packet << 4) | csum;
|
||||||
|
|
||||||
return packet;
|
return packet;
|
||||||
@ -1014,6 +1048,22 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// send dshot for all groups that support it
|
||||||
|
void RCOutput::dshot_send_groups(bool blocking)
|
||||||
|
{
|
||||||
|
for (uint8_t i = 0; i < NUM_GROUPS; i++) {
|
||||||
|
pwm_group &group = pwm_group_list[i];
|
||||||
|
if (group.can_send_dshot_pulse()) {
|
||||||
|
dshot_send(group, blocking);
|
||||||
|
// delay sending the next group by the same amount as the bidir dead time
|
||||||
|
// to avoid irq collisions
|
||||||
|
if (group.bdshot.enabled) {
|
||||||
|
hal.scheduler->delay_microseconds(group.dshot_pulse_time_us);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
send a set of DShot packets for a channel group
|
send a set of DShot packets for a channel group
|
||||||
This call be called in blocking mode from the timer, in which case it waits for the DMA lock.
|
This call be called in blocking mode from the timer, in which case it waits for the DMA lock.
|
||||||
@ -1022,26 +1072,72 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t
|
|||||||
void RCOutput::dshot_send(pwm_group &group, bool blocking)
|
void RCOutput::dshot_send(pwm_group &group, bool blocking)
|
||||||
{
|
{
|
||||||
#ifndef DISABLE_DSHOT
|
#ifndef DISABLE_DSHOT
|
||||||
if (irq.waiter) {
|
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
|
||||||
// doing serial output, don't send DShot pulses
|
// doing serial output or DMAR input, don't send DShot pulses
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// first make sure we have the DMA channel before anything else
|
||||||
if (blocking) {
|
if (blocking) {
|
||||||
group.dma_handle->lock();
|
group.dma_handle->lock();
|
||||||
} else {
|
} else if (!group.dma_handle->lock_nonblock()) {
|
||||||
if (!group.dma_handle->lock_nonblock()) {
|
return;
|
||||||
return;
|
}
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
// assume that we won't be able to get the input capture lock
|
||||||
|
group.bdshot.enabled = false;
|
||||||
|
|
||||||
|
// now grab the input capture lock if we are able
|
||||||
|
if ((_bdshot.mask & (1 << group.chan[group.bdshot.curr_telem_chan])) && group.has_ic()) {
|
||||||
|
if (group.has_shared_ic_up_dma()) {
|
||||||
|
// no locking required
|
||||||
|
group.bdshot.enabled = true;
|
||||||
|
} else if (blocking) {
|
||||||
|
group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->lock();
|
||||||
|
group.bdshot.enabled = true;
|
||||||
|
} else {
|
||||||
|
group.bdshot.enabled = group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->lock_nonblock();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// if the last transaction returned telemetry, decode it
|
||||||
|
if (group.dshot_state == DshotState::RECV_COMPLETE) {
|
||||||
|
uint8_t chan = group.chan[group.bdshot.prev_telem_chan];
|
||||||
|
uint32_t now = AP_HAL::millis();
|
||||||
|
if (bdshot_decode_dshot_telemetry(group, group.bdshot.prev_telem_chan)) {
|
||||||
|
_bdshot.erpm_clean_frames[chan]++;
|
||||||
|
} else {
|
||||||
|
_bdshot.erpm_errors[chan]++;
|
||||||
|
}
|
||||||
|
// reset statistics periodically
|
||||||
|
if (now - _bdshot.erpm_last_stats_ms[chan] > 5000) {
|
||||||
|
_bdshot.erpm_clean_frames[chan] = 0;
|
||||||
|
_bdshot.erpm_errors[chan] = 0;
|
||||||
|
_bdshot.erpm_last_stats_ms[chan] = now;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (group.bdshot.enabled) {
|
||||||
|
if (group.pwm_started) {
|
||||||
|
pwmStop(group.pwm_drv);
|
||||||
|
}
|
||||||
|
pwmStart(group.pwm_drv, &group.pwm_cfg);
|
||||||
|
group.pwm_started = true;
|
||||||
|
|
||||||
|
// we can be more precise for capture timer
|
||||||
|
group.bdshot.telempsc = (uint16_t)(lrintf(((float)group.pwm_drv->clock / bdshot_get_output_rate_hz(group.current_mode) + 0.01f)/TELEM_IC_SAMPLE) - 1);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
|
bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
|
||||||
|
|
||||||
memset((uint8_t *)group.dma_buffer, 0, dshot_buffer_length);
|
memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
|
||||||
|
|
||||||
for (uint8_t i=0; i<4; i++) {
|
for (uint8_t i=0; i<4; i++) {
|
||||||
uint8_t chan = group.chan[i];
|
uint8_t chan = group.chan[i];
|
||||||
if (chan != CHAN_DISABLED) {
|
if (chan != CHAN_DISABLED) {
|
||||||
|
// retrieve the last erpm values
|
||||||
|
_bdshot.erpm[chan] = group.bdshot.erpm[i];
|
||||||
|
|
||||||
uint16_t pwm = period[chan];
|
uint16_t pwm = period[chan];
|
||||||
|
|
||||||
if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
|
if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
|
||||||
@ -1075,7 +1171,7 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool request_telemetry = (telem_request_mask & chan_mask)?true:false;
|
bool request_telemetry = (telem_request_mask & chan_mask)?true:false;
|
||||||
uint16_t packet = create_dshot_packet(value, request_telemetry);
|
uint16_t packet = create_dshot_packet(value, request_telemetry, _bdshot.mask);
|
||||||
if (request_telemetry) {
|
if (request_telemetry) {
|
||||||
telem_request_mask &= ~chan_mask;
|
telem_request_mask &= ~chan_mask;
|
||||||
}
|
}
|
||||||
@ -1084,13 +1180,12 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// start sending the pulses out
|
// start sending the pulses out
|
||||||
send_pulses_DMAR(group, dshot_buffer_length);
|
send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH);
|
||||||
|
|
||||||
group.last_dmar_send_us = AP_HAL::micros64();
|
group.last_dmar_send_us = AP_HAL::micros64();
|
||||||
#endif //#ifndef DISABLE_DSHOT
|
#endif //#ifndef DISABLE_DSHOT
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
send a set of Serial LED packets for a channel group
|
send a set of Serial LED packets for a channel group
|
||||||
return true if send was successful
|
return true if send was successful
|
||||||
@ -1132,6 +1227,8 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
|
|||||||
datasheet. Many thanks to the betaflight developers for coming
|
datasheet. Many thanks to the betaflight developers for coming
|
||||||
up with this great method.
|
up with this great method.
|
||||||
*/
|
*/
|
||||||
|
TOGGLE_PIN_DEBUG(54);
|
||||||
|
|
||||||
dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR));
|
dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR));
|
||||||
stm32_cacheBufferFlush(group.dma_buffer, buffer_length);
|
stm32_cacheBufferFlush(group.dma_buffer, buffer_length);
|
||||||
dmaStreamSetMemory0(group.dma, group.dma_buffer);
|
dmaStreamSetMemory0(group.dma, group.dma_buffer);
|
||||||
@ -1146,28 +1243,33 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
|
|||||||
// setup for 4 burst strided transfers. 0x0D is the register
|
// setup for 4 burst strided transfers. 0x0D is the register
|
||||||
// address offset of the CCR registers in the timer peripheral
|
// address offset of the CCR registers in the timer peripheral
|
||||||
group.pwm_drv->tim->DCR = 0x0D | STM32_TIM_DCR_DBL(3);
|
group.pwm_drv->tim->DCR = 0x0D | STM32_TIM_DCR_DBL(3);
|
||||||
|
group.dshot_state = DshotState::SEND_START;
|
||||||
|
|
||||||
|
TOGGLE_PIN_DEBUG(54);
|
||||||
|
|
||||||
dmaStreamEnable(group.dma);
|
dmaStreamEnable(group.dma);
|
||||||
#endif //#ifndef DISABLE_DSHOT
|
#endif //#ifndef DISABLE_DSHOT
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
unlock DMA channel after a dshot send completes
|
unlock DMA channel after a dshot send completes and no return value is expected
|
||||||
*/
|
*/
|
||||||
void RCOutput::dma_unlock(void *p)
|
void RCOutput::dma_unlock(void *p)
|
||||||
{
|
{
|
||||||
#if STM32_DMA_ADVANCED
|
|
||||||
pwm_group *group = (pwm_group *)p;
|
|
||||||
chSysLockFromISR();
|
chSysLockFromISR();
|
||||||
|
pwm_group *group = (pwm_group *)p;
|
||||||
|
|
||||||
|
group->dshot_state = DshotState::IDLE;
|
||||||
group->dma_handle->unlock_from_IRQ();
|
group->dma_handle->unlock_from_IRQ();
|
||||||
|
|
||||||
chSysUnlockFromISR();
|
chSysUnlockFromISR();
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifndef HAL_WITH_BIDIR_DSHOT
|
||||||
/*
|
/*
|
||||||
DMA interrupt handler. Used to mark DMA completed for DShot
|
DMA interrupt handler. Used to mark DMA completed for DShot
|
||||||
*/
|
*/
|
||||||
void RCOutput::dma_irq_callback(void *p, uint32_t flags)
|
void RCOutput::dma_up_irq_callback(void *p, uint32_t flags)
|
||||||
{
|
{
|
||||||
pwm_group *group = (pwm_group *)p;
|
pwm_group *group = (pwm_group *)p;
|
||||||
chSysLockFromISR();
|
chSysLockFromISR();
|
||||||
@ -1177,10 +1279,11 @@ void RCOutput::dma_irq_callback(void *p, uint32_t flags)
|
|||||||
chEvtSignalI(irq.waiter, serial_event_mask);
|
chEvtSignalI(irq.waiter, serial_event_mask);
|
||||||
} else {
|
} else {
|
||||||
// this prevents us ever having two dshot pulses too close together
|
// this prevents us ever having two dshot pulses too close together
|
||||||
chVTSetI(&group->dma_timeout, chTimeUS2I(dshot_min_gap_us), dma_unlock, p);
|
chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_time_us + 40), dma_unlock, p);
|
||||||
}
|
}
|
||||||
chSysUnlockFromISR();
|
chSysUnlockFromISR();
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup for serial output to an ESC using the given
|
setup for serial output to an ESC using the given
|
||||||
@ -1230,7 +1333,7 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint16_t cha
|
|||||||
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
||||||
pwm_group &group = pwm_group_list[i];
|
pwm_group &group = pwm_group_list[i];
|
||||||
if (group.ch_mask & chanmask) {
|
if (group.ch_mask & chanmask) {
|
||||||
if (!setup_group_DMA(group, baudrate, 10, false, dshot_buffer_length, false)) {
|
if (!setup_group_DMA(group, baudrate, 10, false, DSHOT_BUFFER_LENGTH, false)) {
|
||||||
serial_end();
|
serial_end();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -1306,12 +1409,12 @@ bool RCOutput::serial_write_byte(uint8_t b)
|
|||||||
*/
|
*/
|
||||||
bool RCOutput::serial_write_bytes(const uint8_t *bytes, uint16_t len)
|
bool RCOutput::serial_write_bytes(const uint8_t *bytes, uint16_t len)
|
||||||
{
|
{
|
||||||
#if STM32_DMA_ADVANCED
|
#ifndef DISABLE_DSHOT
|
||||||
if (!serial_group) {
|
if (!serial_group) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
serial_group->dma_handle->lock();
|
serial_group->dma_handle->lock();
|
||||||
memset(serial_group->dma_buffer, 0, dshot_buffer_length);
|
memset(serial_group->dma_buffer, 0, DSHOT_BUFFER_LENGTH);
|
||||||
while (len--) {
|
while (len--) {
|
||||||
if (!serial_write_byte(*bytes++)) {
|
if (!serial_write_byte(*bytes++)) {
|
||||||
serial_group->dma_handle->unlock();
|
serial_group->dma_handle->unlock();
|
||||||
@ -1327,7 +1430,7 @@ bool RCOutput::serial_write_bytes(const uint8_t *bytes, uint16_t len)
|
|||||||
return true;
|
return true;
|
||||||
#else
|
#else
|
||||||
return false;
|
return false;
|
||||||
#endif //#if STM32_DMA_ADVANCED
|
#endif // DISABLE_DSHOT
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -1650,7 +1753,7 @@ void RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
|
|||||||
/*
|
/*
|
||||||
true when the output mode is of type dshot
|
true when the output mode is of type dshot
|
||||||
*/
|
*/
|
||||||
bool RCOutput::is_dshot_protocol(const enum output_mode mode) const
|
bool RCOutput::is_dshot_protocol(const enum output_mode mode)
|
||||||
{
|
{
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case MODE_PWM_DSHOT150:
|
case MODE_PWM_DSHOT150:
|
||||||
@ -1666,7 +1769,7 @@ bool RCOutput::is_dshot_protocol(const enum output_mode mode) const
|
|||||||
/*
|
/*
|
||||||
returns the bitrate in Hz of the given output_mode
|
returns the bitrate in Hz of the given output_mode
|
||||||
*/
|
*/
|
||||||
uint32_t RCOutput::protocol_bitrate(const enum output_mode mode) const
|
uint32_t RCOutput::protocol_bitrate(const enum output_mode mode)
|
||||||
{
|
{
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case MODE_PWM_DSHOT150:
|
case MODE_PWM_DSHOT150:
|
||||||
@ -1796,7 +1899,6 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup serial LED output data for a given output channel
|
setup serial LED output data for a given output channel
|
||||||
and a LED number. LED -1 is all LEDs
|
and a LED number. LED -1 is all LEDs
|
||||||
|
@ -27,6 +27,8 @@
|
|||||||
#define DISABLE_DSHOT
|
#define DISABLE_DSHOT
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define RCOU_DSHOT_TIMING_DEBUG 0
|
||||||
|
|
||||||
class ChibiOS::RCOutput : public AP_HAL::RCOutput {
|
class ChibiOS::RCOutput : public AP_HAL::RCOutput {
|
||||||
public:
|
public:
|
||||||
void init() override;
|
void init() override;
|
||||||
@ -48,6 +50,13 @@ public:
|
|||||||
max_pwm = _esc_pwm_max;
|
max_pwm = _esc_pwm_max;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
// surface dshot telemetry for use by the harmonic notch and status information
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
uint16_t get_erpm(uint8_t chan) const override { return _bdshot.erpm[chan]; }
|
||||||
|
float get_erpm_error_rate(uint8_t chan) const override {
|
||||||
|
return 100.0f * float(_bdshot.erpm_errors[chan]) / (1 + _bdshot.erpm_errors[chan] + _bdshot.erpm_clean_frames[chan]);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
void set_output_mode(uint16_t mask, const enum output_mode mode) override;
|
void set_output_mode(uint16_t mask, const enum output_mode mode) override;
|
||||||
bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;
|
bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;
|
||||||
|
|
||||||
@ -132,6 +141,14 @@ public:
|
|||||||
*/
|
*/
|
||||||
void set_telem_request_mask(uint16_t mask) override { telem_request_mask = (mask >> chan_offset); }
|
void set_telem_request_mask(uint16_t mask) override { telem_request_mask = (mask >> chan_offset); }
|
||||||
|
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
/*
|
||||||
|
enable bi-directional telemetry request for a mask of channels. This is used
|
||||||
|
with DShot to get telemetry feedback
|
||||||
|
*/
|
||||||
|
void set_bidir_dshot_mask(uint16_t mask) override;
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
get safety switch state, used by Util.cpp
|
get safety switch state, used by Util.cpp
|
||||||
*/
|
*/
|
||||||
@ -174,6 +191,27 @@ public:
|
|||||||
void serial_led_send(const uint16_t chan) override;
|
void serial_led_send(const uint16_t chan) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
enum class DshotState {
|
||||||
|
IDLE = 0,
|
||||||
|
SEND_START = 1,
|
||||||
|
SEND_COMPLETE = 2,
|
||||||
|
RECV_START = 3,
|
||||||
|
RECV_COMPLETE = 4
|
||||||
|
};
|
||||||
|
|
||||||
|
/*
|
||||||
|
DShot handling
|
||||||
|
*/
|
||||||
|
// the pre-bit is needed with TIM5, or we can get some corrupt frames
|
||||||
|
static const uint8_t dshot_pre = 1;
|
||||||
|
static const uint8_t dshot_post = 2;
|
||||||
|
static const uint16_t dshot_bit_length = 16 + dshot_pre + dshot_post;
|
||||||
|
static const uint16_t DSHOT_BUFFER_LENGTH = dshot_bit_length * 4 * sizeof(uint32_t);
|
||||||
|
static const uint16_t MIN_GCR_BIT_LEN = 7;
|
||||||
|
static const uint16_t MAX_GCR_BIT_LEN = 22;
|
||||||
|
static const uint16_t GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN;
|
||||||
|
static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(uint32_t);
|
||||||
|
|
||||||
struct pwm_group {
|
struct pwm_group {
|
||||||
// only advanced timers can do high clocks needed for more than 400Hz
|
// only advanced timers can do high clocks needed for more than 400Hz
|
||||||
bool advanced_timer;
|
bool advanced_timer;
|
||||||
@ -183,6 +221,13 @@ private:
|
|||||||
bool have_up_dma; // can we do DMAR outputs for DShot?
|
bool have_up_dma; // can we do DMAR outputs for DShot?
|
||||||
uint8_t dma_up_stream_id;
|
uint8_t dma_up_stream_id;
|
||||||
uint8_t dma_up_channel;
|
uint8_t dma_up_channel;
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
struct {
|
||||||
|
bool have_dma;
|
||||||
|
uint8_t stream_id;
|
||||||
|
uint8_t channel;
|
||||||
|
} dma_ch[4];
|
||||||
|
#endif
|
||||||
uint8_t alt_functions[4];
|
uint8_t alt_functions[4];
|
||||||
ioline_t pal_lines[4];
|
ioline_t pal_lines[4];
|
||||||
|
|
||||||
@ -200,6 +245,8 @@ private:
|
|||||||
uint32_t rc_frequency;
|
uint32_t rc_frequency;
|
||||||
bool in_serial_dma;
|
bool in_serial_dma;
|
||||||
uint64_t last_dmar_send_us;
|
uint64_t last_dmar_send_us;
|
||||||
|
uint32_t dshot_pulse_time_us;
|
||||||
|
uint32_t dshot_pulse_send_time_us;
|
||||||
virtual_timer_t dma_timeout;
|
virtual_timer_t dma_timeout;
|
||||||
uint8_t serial_nleds;
|
uint8_t serial_nleds;
|
||||||
uint8_t clock_mask;
|
uint8_t clock_mask;
|
||||||
@ -217,6 +264,58 @@ private:
|
|||||||
// thread waiting for byte to be written
|
// thread waiting for byte to be written
|
||||||
thread_t *waiter;
|
thread_t *waiter;
|
||||||
} serial;
|
} serial;
|
||||||
|
|
||||||
|
// support for bi-directional dshot
|
||||||
|
volatile DshotState dshot_state;
|
||||||
|
|
||||||
|
struct {
|
||||||
|
uint16_t erpm[4];
|
||||||
|
volatile bool enabled;
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
const stm32_dma_stream_t *ic_dma[4];
|
||||||
|
uint16_t dma_tx_size; // save tx value from last read
|
||||||
|
Shared_DMA *ic_dma_handle[4];
|
||||||
|
uint8_t telem_tim_ch[4];
|
||||||
|
uint8_t curr_telem_chan;
|
||||||
|
uint8_t prev_telem_chan;
|
||||||
|
uint16_t telempsc;
|
||||||
|
uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
|
||||||
|
#if RCOU_DSHOT_TIMING_DEBUG
|
||||||
|
uint16_t telem_rate[4];
|
||||||
|
uint16_t telem_err_rate[4];
|
||||||
|
uint64_t last_print; // debug
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
} bdshot;
|
||||||
|
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
// do we have an input capture dma channel
|
||||||
|
bool has_ic_dma() const {
|
||||||
|
return bdshot.ic_dma_handle[bdshot.curr_telem_chan] != nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool has_shared_ic_up_dma() const {
|
||||||
|
return bdshot.ic_dma_handle[bdshot.curr_telem_chan] == dma_handle;
|
||||||
|
}
|
||||||
|
|
||||||
|
// is input capture currently enabled
|
||||||
|
bool ic_dma_enabled() const {
|
||||||
|
return bdshot.enabled && has_ic_dma() && bdshot.ic_dma[bdshot.curr_telem_chan] != nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool has_ic() const {
|
||||||
|
return has_ic_dma() || has_shared_ic_up_dma();
|
||||||
|
}
|
||||||
|
|
||||||
|
// do we have any kind of input capture
|
||||||
|
bool ic_enabled() const {
|
||||||
|
return bdshot.enabled && has_ic();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
// are we safe to send another pulse?
|
||||||
|
bool can_send_dshot_pulse() const {
|
||||||
|
return is_dshot_protocol(current_mode) && AP_HAL::micros64() - last_dmar_send_us > (dshot_pulse_time_us + 50);
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -259,6 +358,7 @@ private:
|
|||||||
tprio_t serial_priority;
|
tprio_t serial_priority;
|
||||||
|
|
||||||
static pwm_group pwm_group_list[];
|
static pwm_group pwm_group_list[];
|
||||||
|
static const uint8_t NUM_GROUPS;
|
||||||
uint16_t _esc_pwm_min;
|
uint16_t _esc_pwm_min;
|
||||||
uint16_t _esc_pwm_max;
|
uint16_t _esc_pwm_max;
|
||||||
|
|
||||||
@ -279,6 +379,17 @@ private:
|
|||||||
// these values are for the local channels. Non-local channels are handled by IOMCU
|
// these values are for the local channels. Non-local channels are handled by IOMCU
|
||||||
uint32_t en_mask;
|
uint32_t en_mask;
|
||||||
uint16_t period[max_channels];
|
uint16_t period[max_channels];
|
||||||
|
// handling of bi-directional dshot
|
||||||
|
struct {
|
||||||
|
uint16_t mask;
|
||||||
|
uint16_t erpm[max_channels];
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
uint16_t erpm_errors[max_channels];
|
||||||
|
uint16_t erpm_clean_frames[max_channels];
|
||||||
|
uint32_t erpm_last_stats_ms[max_channels];
|
||||||
|
#endif
|
||||||
|
} _bdshot;
|
||||||
|
|
||||||
uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
|
uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
|
||||||
bool corked;
|
bool corked;
|
||||||
// mask of channels that are running in high speed
|
// mask of channels that are running in high speed
|
||||||
@ -301,6 +412,8 @@ private:
|
|||||||
// iomcu output mode (pwm, oneshot or oneshot125)
|
// iomcu output mode (pwm, oneshot or oneshot125)
|
||||||
enum output_mode iomcu_mode = MODE_PWM_NORMAL;
|
enum output_mode iomcu_mode = MODE_PWM_NORMAL;
|
||||||
|
|
||||||
|
bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
|
||||||
|
|
||||||
// find a channel group given a channel number
|
// find a channel group given a channel number
|
||||||
struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);
|
struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);
|
||||||
|
|
||||||
@ -326,16 +439,6 @@ private:
|
|||||||
// update safety switch and LED
|
// update safety switch and LED
|
||||||
void safety_update(void);
|
void safety_update(void);
|
||||||
|
|
||||||
/*
|
|
||||||
DShot handling
|
|
||||||
*/
|
|
||||||
// the pre-bit is needed with TIM5, or we can get some corrupt frames
|
|
||||||
const uint8_t dshot_pre = 1;
|
|
||||||
const uint8_t dshot_post = 2;
|
|
||||||
const uint16_t dshot_bit_length = 16 + dshot_pre + dshot_post;
|
|
||||||
const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
|
|
||||||
static const uint16_t dshot_min_gap_us = 100;
|
|
||||||
uint32_t dshot_pulse_time_us;
|
|
||||||
uint16_t telem_request_mask;
|
uint16_t telem_request_mask;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -347,17 +450,34 @@ private:
|
|||||||
|
|
||||||
void dma_allocate(Shared_DMA *ctx);
|
void dma_allocate(Shared_DMA *ctx);
|
||||||
void dma_deallocate(Shared_DMA *ctx);
|
void dma_deallocate(Shared_DMA *ctx);
|
||||||
uint16_t create_dshot_packet(const uint16_t value, bool telem_request);
|
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
|
||||||
void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
|
void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
|
||||||
|
|
||||||
|
void dshot_send_groups(bool blocking);
|
||||||
void dshot_send(pwm_group &group, bool blocking);
|
void dshot_send(pwm_group &group, bool blocking);
|
||||||
static void dma_irq_callback(void *p, uint32_t flags);
|
static void dma_up_irq_callback(void *p, uint32_t flags);
|
||||||
static void dma_unlock(void *p);
|
static void dma_unlock(void *p);
|
||||||
bool mode_requires_dma(enum output_mode mode) const;
|
bool mode_requires_dma(enum output_mode mode) const;
|
||||||
bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length, bool choose_high);
|
bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length, bool choose_high);
|
||||||
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
|
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
|
||||||
void set_group_mode(pwm_group &group);
|
void set_group_mode(pwm_group &group);
|
||||||
bool is_dshot_protocol(const enum output_mode mode) const;
|
static bool is_dshot_protocol(const enum output_mode mode);
|
||||||
uint32_t protocol_bitrate(const enum output_mode mode) const;
|
static uint32_t protocol_bitrate(const enum output_mode mode);
|
||||||
|
|
||||||
|
/*
|
||||||
|
Support for bi-direction dshot
|
||||||
|
*/
|
||||||
|
void bdshot_ic_dma_allocate(Shared_DMA *ctx);
|
||||||
|
void bdshot_ic_dma_deallocate(Shared_DMA *ctx);
|
||||||
|
static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count);
|
||||||
|
static bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
|
||||||
|
static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
|
||||||
|
static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags);
|
||||||
|
static void bdshot_finish_dshot_gcr_transaction(void *p);
|
||||||
|
bool bdshot_setup_group_ic_DMA(pwm_group &group);
|
||||||
|
static void bdshot_receive_pulses_DMAR(pwm_group* group);
|
||||||
|
static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch);
|
||||||
|
static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup neopixel (WS2812B) output data for a given output channel
|
setup neopixel (WS2812B) output data for a given output channel
|
||||||
|
575
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
Normal file
575
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
Normal file
@ -0,0 +1,575 @@
|
|||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*
|
||||||
|
* Bi-directional dshot based on Betaflight, code by Andy Piper and Siddharth Bharat Purohit
|
||||||
|
*/
|
||||||
|
#include "RCOutput.h"
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include "hwdef/common/stm32_util.h"
|
||||||
|
#include <AP_InternalError/AP_InternalError.h>
|
||||||
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
|
|
||||||
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||||
|
|
||||||
|
using namespace ChibiOS;
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
#if RCOU_DSHOT_TIMING_DEBUG
|
||||||
|
#define DEBUG_CHANNEL 1
|
||||||
|
#define TOGGLE_PIN_CH_DEBUG(pin, channel) do { if (channel == DEBUG_CHANNEL) palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
|
||||||
|
#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
|
||||||
|
#else
|
||||||
|
#define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0)
|
||||||
|
#define TOGGLE_PIN_DEBUG(pin) do {} while (0)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define TELEM_IC_SAMPLE 16
|
||||||
|
|
||||||
|
// marker for a disabled channel
|
||||||
|
#define CHAN_DISABLED 255
|
||||||
|
|
||||||
|
// #pragma GCC optimize("Og")
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* enable bi-directional telemetry request for a mask of channels. This is used
|
||||||
|
* with DShot to get telemetry feedback
|
||||||
|
*/
|
||||||
|
void RCOutput::set_bidir_dshot_mask(uint16_t mask)
|
||||||
|
{
|
||||||
|
_bdshot.mask = (mask >> chan_offset);
|
||||||
|
// we now need to reconfigure the DMA channels since they are affected by the value of the mask
|
||||||
|
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
||||||
|
pwm_group &group = pwm_group_list[i];
|
||||||
|
if (((group.ch_mask << chan_offset) & mask) == 0) {
|
||||||
|
// this group is not affected
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
set_group_mode(group);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
|
||||||
|
{
|
||||||
|
bool set_curr_chan = false;
|
||||||
|
|
||||||
|
for (uint8_t i = 0; i < 4; i++) {
|
||||||
|
if (group.chan[i] == CHAN_DISABLED ||
|
||||||
|
!group.dma_ch[i].have_dma || !(_bdshot.mask & (1 << group.chan[i]))) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
// make sure we don't start on a disabled channel
|
||||||
|
if (!set_curr_chan) {
|
||||||
|
group.bdshot.curr_telem_chan = i;
|
||||||
|
set_curr_chan = true;
|
||||||
|
}
|
||||||
|
pwmmode_t mode = group.pwm_cfg.channels[i].mode;
|
||||||
|
if (mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW ||
|
||||||
|
mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH) {
|
||||||
|
// Complementary channels don't support input capture
|
||||||
|
// Return error
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!group.bdshot.ic_dma_handle[i]) {
|
||||||
|
// share up channel if required
|
||||||
|
if (group.dma_ch[i].stream_id == group.dma_up_stream_id) {
|
||||||
|
group.bdshot.ic_dma_handle[i] = group.dma_handle;
|
||||||
|
} else {
|
||||||
|
group.bdshot.ic_dma_handle[i] = new Shared_DMA(group.dma_ch[i].stream_id, SHARED_DMA_NONE,
|
||||||
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_allocate, void, Shared_DMA *),
|
||||||
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *));
|
||||||
|
}
|
||||||
|
if (!group.bdshot.ic_dma_handle[i]) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// We might need to do sharing of timers for telemetry feedback
|
||||||
|
// due to lack of available DMA channels
|
||||||
|
for (uint8_t i = 0; i < 4; i++) {
|
||||||
|
if (group.chan[i] == CHAN_DISABLED || !(_bdshot.mask & (1 << group.chan[i]))) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
uint8_t curr_chan = i;
|
||||||
|
if (group.bdshot.ic_dma_handle[i]) {
|
||||||
|
// we are all good just set and continue
|
||||||
|
group.bdshot.telem_tim_ch[i] = curr_chan;
|
||||||
|
} else {
|
||||||
|
// I guess we have to share, but only channels 1 & 2 or 3 & 4
|
||||||
|
if (curr_chan % 2 == 0) {
|
||||||
|
curr_chan = curr_chan + 1;
|
||||||
|
} else {
|
||||||
|
curr_chan = curr_chan - 1;
|
||||||
|
}
|
||||||
|
if (!group.dma_ch[curr_chan].have_dma) {
|
||||||
|
// We can't find a DMA channel to use so
|
||||||
|
// return error
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (group.bdshot.ic_dma_handle[i]) {
|
||||||
|
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
// share up channel if required
|
||||||
|
if (group.dma_ch[curr_chan].stream_id == group.dma_up_stream_id) {
|
||||||
|
group.bdshot.ic_dma_handle[i] = group.dma_handle;
|
||||||
|
} else {
|
||||||
|
// we can use the next channel
|
||||||
|
group.bdshot.ic_dma_handle[i] = new Shared_DMA(group.dma_ch[curr_chan].stream_id, SHARED_DMA_NONE,
|
||||||
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_allocate, void, Shared_DMA *),
|
||||||
|
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *));
|
||||||
|
}
|
||||||
|
if (!group.bdshot.ic_dma_handle[i]) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
group.bdshot.telem_tim_ch[i] = curr_chan;
|
||||||
|
group.dma_ch[i] = group.dma_ch[curr_chan];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
allocate DMA channel
|
||||||
|
*/
|
||||||
|
void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
|
||||||
|
{
|
||||||
|
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
||||||
|
pwm_group &group = pwm_group_list[i];
|
||||||
|
for (uint8_t icuch = 0; icuch < 4; icuch++) {
|
||||||
|
if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] == nullptr) {
|
||||||
|
chSysLock();
|
||||||
|
group.bdshot.ic_dma[icuch] = dmaStreamAllocI(group.dma_ch[icuch].stream_id, 10, bdshot_dma_ic_irq_callback, &group);
|
||||||
|
chSysUnlock();
|
||||||
|
#if STM32_DMA_SUPPORTS_DMAMUX
|
||||||
|
if (group.bdshot.ic_dma[icuch]) {
|
||||||
|
dmaSetRequestSource(group.bdshot.ic_dma[icuch], group.dma_ch[icuch].channel);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
deallocate DMA channel
|
||||||
|
*/
|
||||||
|
void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx)
|
||||||
|
{
|
||||||
|
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
|
||||||
|
pwm_group &group = pwm_group_list[i];
|
||||||
|
for (uint8_t icuch = 0; icuch < 4; icuch++) {
|
||||||
|
if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] != nullptr) {
|
||||||
|
chSysLock();
|
||||||
|
dmaStreamFreeI(group.bdshot.ic_dma[icuch]);
|
||||||
|
group.bdshot.ic_dma[icuch] = nullptr;
|
||||||
|
chSysUnlock();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
|
||||||
|
// called from the interrupt
|
||||||
|
void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
|
||||||
|
{
|
||||||
|
// make sure the transaction finishes or times out, this function takes a little time to run so the most
|
||||||
|
// accurate timing is from the beginning. the pulse time is slightly longer than we need so an extra 10U
|
||||||
|
// should be plenty
|
||||||
|
chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 10U),
|
||||||
|
bdshot_finish_dshot_gcr_transaction, group);
|
||||||
|
uint8_t curr_ch = group->bdshot.curr_telem_chan;
|
||||||
|
|
||||||
|
// Configure Timer
|
||||||
|
group->pwm_drv->tim->SR = 0;
|
||||||
|
group->pwm_drv->tim->CCER = 0;
|
||||||
|
group->pwm_drv->tim->CCMR1 = 0;
|
||||||
|
group->pwm_drv->tim->CCMR2 = 0;
|
||||||
|
group->pwm_drv->tim->CCER = 0;
|
||||||
|
group->pwm_drv->tim->DIER = 0;
|
||||||
|
group->pwm_drv->tim->CR1 = 0;
|
||||||
|
group->pwm_drv->tim->CR2 = 0;
|
||||||
|
group->pwm_drv->tim->PSC = group->bdshot.telempsc;
|
||||||
|
|
||||||
|
group->dshot_state = DshotState::RECV_START;
|
||||||
|
|
||||||
|
//TOGGLE_PIN_CH_DEBUG(54, curr_ch);
|
||||||
|
group->pwm_drv->tim->ARR = 0xFFFF; // count forever
|
||||||
|
group->pwm_drv->tim->CNT = 0;
|
||||||
|
|
||||||
|
// Initialise ICU channels
|
||||||
|
bdshot_config_icu_dshot(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]);
|
||||||
|
|
||||||
|
uint32_t ic_channel = STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel);
|
||||||
|
// do a little DMA dance when sharing with UP
|
||||||
|
if (group->has_shared_ic_up_dma()) {
|
||||||
|
ic_channel = STM32_DMA_CR_CHSEL(group->dma_up_channel);
|
||||||
|
}
|
||||||
|
const stm32_dma_stream_t *ic_dma =
|
||||||
|
group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_ch];
|
||||||
|
|
||||||
|
// Configure DMA
|
||||||
|
dmaStreamSetPeripheral(ic_dma, &(group->pwm_drv->tim->DMAR));
|
||||||
|
dmaStreamSetMemory0(ic_dma, group->dma_buffer);
|
||||||
|
dmaStreamSetTransactionSize(ic_dma, GCR_TELEMETRY_BIT_LEN);
|
||||||
|
dmaStreamSetFIFO(ic_dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
|
||||||
|
dmaStreamSetMode(ic_dma,
|
||||||
|
ic_channel |
|
||||||
|
STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD |
|
||||||
|
STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) |
|
||||||
|
STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE);
|
||||||
|
|
||||||
|
// setup for transfers. 0x0D is the register
|
||||||
|
// address offset of the CCR registers in the timer peripheral
|
||||||
|
group->pwm_drv->tim->DCR = (0x0D + group->bdshot.telem_tim_ch[curr_ch]) | STM32_TIM_DCR_DBL(0);
|
||||||
|
|
||||||
|
// Start Timer
|
||||||
|
group->pwm_drv->tim->EGR |= STM32_TIM_EGR_UG;
|
||||||
|
group->pwm_drv->tim->SR = 0;
|
||||||
|
group->pwm_drv->tim->CR1 = TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_UDIS | STM32_TIM_CR1_CEN;
|
||||||
|
dmaStreamEnable(ic_dma);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch)
|
||||||
|
{
|
||||||
|
switch(ccr_ch) {
|
||||||
|
case 0: {
|
||||||
|
/* Disable the Channel 1: Reset the CC1E Bit */
|
||||||
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC1E;
|
||||||
|
|
||||||
|
const uint32_t CCMR1_FILT = TIM_CCMR1_IC1F_1; // 4 samples per output transition
|
||||||
|
// Select the Input and set the filter and the prescaler value
|
||||||
|
if (chan == 0) {
|
||||||
|
MODIFY_REG(TIMx->CCMR1,
|
||||||
|
(TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
|
||||||
|
(TIM_CCMR1_CC1S_0 | CCMR1_FILT));
|
||||||
|
} else {
|
||||||
|
MODIFY_REG(TIMx->CCMR1,
|
||||||
|
(TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
|
||||||
|
(TIM_CCMR1_CC1S_1 | CCMR1_FILT));
|
||||||
|
}
|
||||||
|
// Select the Polarity as Both Edge and set the CC1E Bit
|
||||||
|
MODIFY_REG(TIMx->CCER,
|
||||||
|
(TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E),
|
||||||
|
(TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E));
|
||||||
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE, TIM_DIER_CC1DE);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 1: {
|
||||||
|
// Disable the Channel 2: Reset the CC2E Bit
|
||||||
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC2E;
|
||||||
|
|
||||||
|
const uint32_t CCMR1_FILT = TIM_CCMR1_IC2F_1;
|
||||||
|
// Select the Input and set the filter and the prescaler value
|
||||||
|
if (chan == 0) {
|
||||||
|
MODIFY_REG(TIMx->CCMR1,
|
||||||
|
(TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
|
||||||
|
(TIM_CCMR1_CC2S_1 | CCMR1_FILT));
|
||||||
|
} else {
|
||||||
|
MODIFY_REG(TIMx->CCMR1,
|
||||||
|
(TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
|
||||||
|
(TIM_CCMR1_CC2S_0 | CCMR1_FILT));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Select the Polarity as Both Edge and set the CC2E Bit
|
||||||
|
MODIFY_REG(TIMx->CCER,
|
||||||
|
TIM_CCER_CC2P | TIM_CCER_CC2NP | TIM_CCER_CC2E,
|
||||||
|
(TIM_CCER_CC2P | TIM_CCER_CC2NP | TIM_CCER_CC2E));
|
||||||
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC2DE, TIM_DIER_CC2DE);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 2: {
|
||||||
|
// Disable the Channel 3: Reset the CC3E Bit
|
||||||
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC3E;
|
||||||
|
|
||||||
|
const uint32_t CCMR2_FILT = TIM_CCMR2_IC3F_1;
|
||||||
|
// Select the Input and set the filter and the prescaler value
|
||||||
|
if (chan == 2) {
|
||||||
|
MODIFY_REG(TIMx->CCMR2,
|
||||||
|
(TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
|
||||||
|
(TIM_CCMR2_CC3S_0 | CCMR2_FILT));
|
||||||
|
} else {
|
||||||
|
MODIFY_REG(TIMx->CCMR2,
|
||||||
|
(TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
|
||||||
|
(TIM_CCMR2_CC3S_1 | CCMR2_FILT));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Select the Polarity as Both Edge and set the CC3E Bit
|
||||||
|
MODIFY_REG(TIMx->CCER,
|
||||||
|
(TIM_CCER_CC3P | TIM_CCER_CC3NP | TIM_CCER_CC3E),
|
||||||
|
(TIM_CCER_CC3P | TIM_CCER_CC3NP | TIM_CCER_CC3E));
|
||||||
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE, TIM_DIER_CC3DE);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 3: {
|
||||||
|
// Disable the Channel 4: Reset the CC4E Bit
|
||||||
|
TIMx->CCER &= (uint32_t)~TIM_CCER_CC4E;
|
||||||
|
|
||||||
|
const uint32_t CCMR2_FILT = TIM_CCMR2_IC4F_1;
|
||||||
|
// Select the Input and set the filter and the prescaler value
|
||||||
|
if (chan == 2) {
|
||||||
|
MODIFY_REG(TIMx->CCMR2,
|
||||||
|
(TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
|
||||||
|
(TIM_CCMR2_CC4S_1 | CCMR2_FILT));
|
||||||
|
} else {
|
||||||
|
MODIFY_REG(TIMx->CCMR2,
|
||||||
|
(TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
|
||||||
|
(TIM_CCMR2_CC4S_0 | CCMR2_FILT));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Select the Polarity as Both Edge and set the CC4E Bit
|
||||||
|
MODIFY_REG(TIMx->CCER,
|
||||||
|
(TIM_CCER_CC4P | TIM_CCER_CC4NP | TIM_CCER_CC4E),
|
||||||
|
(TIM_CCER_CC4P | TIM_CCER_CC4NP | TIM_CCER_CC4E));
|
||||||
|
|
||||||
|
MODIFY_REG(TIMx->DIER, TIM_DIER_CC4DE, TIM_DIER_CC4DE);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
unlock DMA channel after a bi-directional dshot transaction completes
|
||||||
|
*/
|
||||||
|
void RCOutput::bdshot_finish_dshot_gcr_transaction(void *p)
|
||||||
|
{
|
||||||
|
pwm_group *group = (pwm_group *)p;
|
||||||
|
chSysLockFromISR();
|
||||||
|
#ifdef HAL_GPIO_LINE_GPIO56
|
||||||
|
TOGGLE_PIN_DEBUG(56);
|
||||||
|
#endif
|
||||||
|
uint8_t curr_telem_chan = group->bdshot.curr_telem_chan;
|
||||||
|
|
||||||
|
const stm32_dma_stream_t *dma =
|
||||||
|
group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_telem_chan];
|
||||||
|
// record the transaction size before the stream is released
|
||||||
|
dmaStreamDisable(dma);
|
||||||
|
group->bdshot.dma_tx_size = MIN(uint16_t(GCR_TELEMETRY_BIT_LEN),
|
||||||
|
GCR_TELEMETRY_BIT_LEN - dmaStreamGetTransactionSize(dma));
|
||||||
|
stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size);
|
||||||
|
memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(uint32_t) * group->bdshot.dma_tx_size);
|
||||||
|
|
||||||
|
group->dshot_state = DshotState::RECV_COMPLETE;
|
||||||
|
|
||||||
|
// if using input capture DMA then clean up
|
||||||
|
if (group->ic_dma_enabled()) {
|
||||||
|
group->bdshot.ic_dma_handle[curr_telem_chan]->unlock_from_IRQ();
|
||||||
|
}
|
||||||
|
|
||||||
|
// rotate to the next input channel
|
||||||
|
group->bdshot.prev_telem_chan = group->bdshot.curr_telem_chan;
|
||||||
|
group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group);
|
||||||
|
group->dma_handle->unlock_from_IRQ();
|
||||||
|
#ifdef HAL_GPIO_LINE_GPIO56
|
||||||
|
TOGGLE_PIN_DEBUG(56);
|
||||||
|
#endif
|
||||||
|
chSysUnlockFromISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
decode returned data from bi-directional dshot
|
||||||
|
*/
|
||||||
|
bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
|
||||||
|
{
|
||||||
|
if (group.chan[chan] == CHAN_DISABLED) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// evaluate dshot telemetry
|
||||||
|
group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size);
|
||||||
|
|
||||||
|
group.dshot_state = DshotState::IDLE;
|
||||||
|
|
||||||
|
#if RCOU_DSHOT_TIMING_DEBUG
|
||||||
|
// Record Stats
|
||||||
|
if (group.bdshot.erpm[chan] != 0xFFFF) {
|
||||||
|
group.bdshot.telem_rate[chan]++;
|
||||||
|
} else {
|
||||||
|
#ifdef HAL_GPIO_LINE_GPIO57
|
||||||
|
TOGGLE_PIN_DEBUG(57);
|
||||||
|
#endif
|
||||||
|
group.bdshot.telem_err_rate[chan]++;
|
||||||
|
#ifdef HAL_GPIO_LINE_GPIO57
|
||||||
|
TOGGLE_PIN_DEBUG(57);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t now = AP_HAL::micros64();
|
||||||
|
if (chan == DEBUG_CHANNEL && (now - group.bdshot.last_print) > 1000000) {
|
||||||
|
hal.console->printf("TELEM: %d <%d Hz, %.1f%% err>", group.bdshot.erpm[chan], group.bdshot.telem_rate[chan],
|
||||||
|
100.0f * float(group.bdshot.telem_err_rate[chan]) / (group.bdshot.telem_err_rate[chan] + group.bdshot.telem_rate[chan]));
|
||||||
|
hal.console->printf(" %ld ", group.bdshot.dma_buffer_copy[0]);
|
||||||
|
for (uint8_t l = 1; l < group.bdshot.dma_tx_size; l++) {
|
||||||
|
hal.console->printf(" +%ld ", group.bdshot.dma_buffer_copy[l] - group.bdshot.dma_buffer_copy[l-1]);
|
||||||
|
}
|
||||||
|
hal.console->printf("\n");
|
||||||
|
|
||||||
|
group.bdshot.telem_rate[chan] = 0;
|
||||||
|
group.bdshot.telem_err_rate[chan] = 0;
|
||||||
|
group.bdshot.last_print = now;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
return group.bdshot.erpm[chan] != 0xFFFF;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Find next valid channel for dshot telem
|
||||||
|
uint8_t RCOutput::bdshot_find_next_ic_channel(const pwm_group& group)
|
||||||
|
{
|
||||||
|
uint8_t chan = group.bdshot.curr_telem_chan;
|
||||||
|
for (uint8_t i = 1; i < 4; i++) {
|
||||||
|
const uint8_t next_chan = (chan + i) % 4;
|
||||||
|
if (group.chan[next_chan] != CHAN_DISABLED &&
|
||||||
|
group.bdshot.ic_dma_handle[next_chan] != nullptr) {
|
||||||
|
return next_chan;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return chan;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
DMA UP channel interrupt handler. Used to mark DMA send completed for DShot
|
||||||
|
*/
|
||||||
|
void RCOutput::dma_up_irq_callback(void *p, uint32_t flags)
|
||||||
|
{
|
||||||
|
pwm_group *group = (pwm_group *)p;
|
||||||
|
chSysLockFromISR();
|
||||||
|
|
||||||
|
// there is a small chance the shared UP CH codepath will get here
|
||||||
|
if (group->bdshot.enabled && group->dshot_state == DshotState::RECV_START) {
|
||||||
|
chSysUnlockFromISR();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check nothing bad happened
|
||||||
|
if ((flags & STM32_DMA_ISR_TEIF) != 0) {
|
||||||
|
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
|
||||||
|
}
|
||||||
|
dmaStreamDisable(group->dma);
|
||||||
|
|
||||||
|
if (group->in_serial_dma && irq.waiter) {
|
||||||
|
// tell the waiting process we've done the DMA
|
||||||
|
chEvtSignalI(irq.waiter, serial_event_mask);
|
||||||
|
} else if (group->bdshot.enabled) {
|
||||||
|
group->dshot_state = DshotState::SEND_COMPLETE;
|
||||||
|
// sending is done, in 30us the ESC will send telemetry
|
||||||
|
TOGGLE_PIN_DEBUG(55);
|
||||||
|
bdshot_receive_pulses_DMAR(group);
|
||||||
|
TOGGLE_PIN_DEBUG(55);
|
||||||
|
} else {
|
||||||
|
// non-bidir case
|
||||||
|
chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_time_us + 40), dma_unlock, p);
|
||||||
|
}
|
||||||
|
|
||||||
|
chSysUnlockFromISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
// DMA IC channel handler. Used to mark DMA receive completed for DShot
|
||||||
|
void RCOutput::bdshot_dma_ic_irq_callback(void *p, uint32_t flags)
|
||||||
|
{
|
||||||
|
chSysLockFromISR();
|
||||||
|
|
||||||
|
// check nothing bad happened
|
||||||
|
if ((flags & STM32_DMA_ISR_TEIF) != 0) {
|
||||||
|
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
|
||||||
|
}
|
||||||
|
|
||||||
|
chSysUnlockFromISR();
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
returns the bitrate in Hz of the given output_mode
|
||||||
|
*/
|
||||||
|
uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode)
|
||||||
|
{
|
||||||
|
switch (mode) {
|
||||||
|
case MODE_PWM_DSHOT150:
|
||||||
|
return 150000U * 5 / 4;
|
||||||
|
case MODE_PWM_DSHOT300:
|
||||||
|
return 300000U * 5 / 4;
|
||||||
|
case MODE_PWM_DSHOT600:
|
||||||
|
return 600000U * 5 / 4;
|
||||||
|
case MODE_PWM_DSHOT1200:
|
||||||
|
return 120000U * 5 / 4;
|
||||||
|
default:
|
||||||
|
// use 1 to prevent a possible divide-by-zero
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket
|
||||||
|
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol
|
||||||
|
uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count)
|
||||||
|
{
|
||||||
|
uint32_t value = 0;
|
||||||
|
uint32_t oldValue = buffer[0];
|
||||||
|
uint32_t bits = 0;
|
||||||
|
uint32_t len;
|
||||||
|
|
||||||
|
for (uint32_t i = 1; i <= count; i++) {
|
||||||
|
if (i < count) {
|
||||||
|
int32_t diff = buffer[i] - oldValue;
|
||||||
|
if (bits >= 21) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
len = (diff + TELEM_IC_SAMPLE/2) / TELEM_IC_SAMPLE;
|
||||||
|
} else {
|
||||||
|
len = 21 - bits;
|
||||||
|
}
|
||||||
|
|
||||||
|
value <<= len;
|
||||||
|
value |= 1 << (len - 1);
|
||||||
|
oldValue = buffer[i];
|
||||||
|
bits += len;
|
||||||
|
}
|
||||||
|
if (bits != 21) {
|
||||||
|
return 0xffff;
|
||||||
|
}
|
||||||
|
|
||||||
|
static const uint32_t decode[32] = {
|
||||||
|
0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
|
||||||
|
0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
|
||||||
|
|
||||||
|
uint32_t decodedValue = decode[value & 0x1f];
|
||||||
|
decodedValue |= decode[(value >> 5) & 0x1f] << 4;
|
||||||
|
decodedValue |= decode[(value >> 10) & 0x1f] << 8;
|
||||||
|
decodedValue |= decode[(value >> 15) & 0x1f] << 12;
|
||||||
|
|
||||||
|
uint32_t csum = decodedValue;
|
||||||
|
csum = csum ^ (csum >> 8); // xor bytes
|
||||||
|
csum = csum ^ (csum >> 4); // xor nibbles
|
||||||
|
|
||||||
|
if ((csum & 0xf) != 0xf) {
|
||||||
|
return 0xffff;
|
||||||
|
}
|
||||||
|
decodedValue >>= 4;
|
||||||
|
|
||||||
|
if (decodedValue == 0x0fff) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
|
||||||
|
if (!decodedValue) {
|
||||||
|
return 0xffff;
|
||||||
|
}
|
||||||
|
uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif // HAL_WITH_BIDIR_DSHOT
|
324
libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/hwdef.dat
Normal file
324
libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/hwdef.dat
Normal file
@ -0,0 +1,324 @@
|
|||||||
|
# hw definition file for processing by chibios_hwdef.py
|
||||||
|
|
||||||
|
# USB setup
|
||||||
|
USB_VENDOR 0x2DAE # ONLY FOR USE BY HEX! NOBODY ELSE
|
||||||
|
USB_PRODUCT 0x1016
|
||||||
|
USB_STRING_MANUFACTURER "Hex/ProfiCNC"
|
||||||
|
USB_STRING_PRODUCT "CubeOrange"
|
||||||
|
USB_STRING_SERIAL "%SERIAL%"
|
||||||
|
|
||||||
|
# MCU class and specific type
|
||||||
|
MCU STM32H7xx STM32H743xx
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 24000000
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 140
|
||||||
|
|
||||||
|
FLASH_SIZE_KB 2048
|
||||||
|
|
||||||
|
# with 2M flash we can afford to optimize for speed
|
||||||
|
env OPTIMIZE -O2
|
||||||
|
|
||||||
|
FLASH_RESERVE_START_KB 128
|
||||||
|
|
||||||
|
define HAL_STORAGE_SIZE 16384
|
||||||
|
|
||||||
|
# order of I2C buses
|
||||||
|
I2C_ORDER I2C2 I2C1
|
||||||
|
|
||||||
|
# order of UARTs (and USB)
|
||||||
|
SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 UART7 OTG2
|
||||||
|
|
||||||
|
# If the board has an IOMCU connected via a UART then this defines the
|
||||||
|
# UART to talk to that MCU. Leave it out for boards with no IOMCU.
|
||||||
|
|
||||||
|
# UART for IOMCU
|
||||||
|
IOMCU_UART USART6
|
||||||
|
|
||||||
|
# UART4 serial GPS
|
||||||
|
PA0 UART4_TX UART4
|
||||||
|
PA1 UART4_RX UART4 NODMA
|
||||||
|
|
||||||
|
PA2 BATT_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PA3 BATT_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
# Now the VDD sense pin. This is used to sense primary board voltage.
|
||||||
|
PA4 VDD_5V_SENS ADC1 SCALE(2)
|
||||||
|
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# This defines an output pin which will default to output HIGH. It is
|
||||||
|
# a pin that enables peripheral power on this board. It starts in the
|
||||||
|
# off state, then is pulled low to enable peripherals in
|
||||||
|
# peripheral_power_enable()
|
||||||
|
PA8 nVDD_5V_PERIPH_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# This is the pin that senses USB being connected. It is an input pin
|
||||||
|
# setup as OPENDRAIN.
|
||||||
|
PA9 VBUS INPUT OPENDRAIN
|
||||||
|
|
||||||
|
# Now we define the pins that USB is connected on.
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
# These are the pins for SWD debugging with a STlinkv2 or black-magic probe.
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
# PWM output for buzzer
|
||||||
|
PA15 TIM2_CH1 TIM2 GPIO(77) ALARM
|
||||||
|
|
||||||
|
# This defines a couple of general purpose outputs, mapped to GPIO
|
||||||
|
# numbers 1 and 2 for users.
|
||||||
|
PB0 EXTERN_GPIO1 OUTPUT GPIO(1)
|
||||||
|
PB1 EXTERN_GPIO2 OUTPUT GPIO(2)
|
||||||
|
|
||||||
|
# This defines some input pins, currently unused.
|
||||||
|
PB2 BOOT1 INPUT
|
||||||
|
PB3 FMU_SW0 INPUT
|
||||||
|
|
||||||
|
# This defines the pins for the 2nd CAN interface, if available.
|
||||||
|
PB6 CAN2_TX CAN2
|
||||||
|
PB12 CAN2_RX CAN2
|
||||||
|
|
||||||
|
# Now the first I2C bus. The pin speeds are automatically setup
|
||||||
|
# correctly, but can be overridden here if needed.
|
||||||
|
PB8 I2C1_SCL I2C1
|
||||||
|
PB9 I2C1_SDA I2C1
|
||||||
|
|
||||||
|
# the 2nd I2C bus
|
||||||
|
PB10 I2C2_SCL I2C2
|
||||||
|
PB11 I2C2_SDA I2C2
|
||||||
|
|
||||||
|
# the 2nd SPI bus
|
||||||
|
PB13 SPI2_SCK SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
|
||||||
|
# This input pin is used to detect that power is valid on USB.
|
||||||
|
PC0 VBUS_nVALID INPUT PULLUP
|
||||||
|
|
||||||
|
# This defines the CS pin for the magnetometer and first IMU. Note
|
||||||
|
# that CS pins are software controlled, and are not tied to a particular
|
||||||
|
# SPI bus.
|
||||||
|
PC1 MAG_CS CS
|
||||||
|
PC2 MPU_CS CS
|
||||||
|
|
||||||
|
# This defines more ADC inputs.
|
||||||
|
PC3 AUX_POWER ADC1 SCALE(1)
|
||||||
|
PC4 AUX_ADC2 ADC1 SCALE(1)
|
||||||
|
|
||||||
|
# And the analog input for airspeed (rarely used these days).
|
||||||
|
PC5 PRESSURE_SENS ADC1 SCALE(2)
|
||||||
|
|
||||||
|
# USART6 to IO
|
||||||
|
PC6 USART6_TX USART6
|
||||||
|
PC7 USART6_RX USART6
|
||||||
|
|
||||||
|
# Now setup the pins for the microSD card, if available.
|
||||||
|
PC8 SDMMC1_D0 SDMMC1
|
||||||
|
PC9 SDMMC1_D1 SDMMC1
|
||||||
|
PC10 SDMMC1_D2 SDMMC1
|
||||||
|
PC11 SDMMC1_D3 SDMMC1
|
||||||
|
PC12 SDMMC1_CK SDMMC1
|
||||||
|
PD2 SDMMC1_CMD SDMMC1
|
||||||
|
|
||||||
|
# More CS pins for more sensors. The labels for all CS pins need to
|
||||||
|
# match the SPI device table later in this file.
|
||||||
|
PC13 GYRO_EXT_CS CS
|
||||||
|
PC14 BARO_EXT_CS CS
|
||||||
|
PC15 ACCEL_EXT_CS CS
|
||||||
|
PD7 BARO_CS CS
|
||||||
|
PE4 MPU_EXT_CS CS
|
||||||
|
|
||||||
|
# the first CAN bus
|
||||||
|
PD0 CAN1_RX CAN1
|
||||||
|
PD1 CAN1_TX CAN1
|
||||||
|
|
||||||
|
# Another USART, this one for telem1. This one has RTS and CTS lines.
|
||||||
|
# USART2 serial2 telem1
|
||||||
|
PD3 USART2_CTS USART2
|
||||||
|
PD4 USART2_RTS USART2
|
||||||
|
PD5 USART2_TX USART2
|
||||||
|
PD6 USART2_RX USART2
|
||||||
|
|
||||||
|
# The telem2 USART, also with RTS/CTS available.
|
||||||
|
# USART3 serial3 telem2
|
||||||
|
PD8 USART3_TX USART3
|
||||||
|
PD9 USART3_RX USART3
|
||||||
|
PD11 USART3_CTS USART3
|
||||||
|
PD12 USART3_RTS USART3
|
||||||
|
|
||||||
|
# The CS pin for FRAM (ramtron). This one is marked as using
|
||||||
|
# SPEED_VERYLOW, which matches the HAL_PX4 setup.
|
||||||
|
PD10 FRAM_CS CS SPEED_VERYLOW
|
||||||
|
|
||||||
|
# Now we start defining some PWM pins. We also map these pins to GPIO
|
||||||
|
# values, so users can set BRD_PWM_COUNT to choose how many of the PWM
|
||||||
|
# outputs on the primary MCU are setup as PWM and how many as
|
||||||
|
# GPIOs. To match HAL_PX4 we number the GPIOs for the PWM outputs
|
||||||
|
# starting at 50.
|
||||||
|
PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) BIDIR
|
||||||
|
PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51) # this will automatically be shared with TIM1_CH4
|
||||||
|
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR
|
||||||
|
PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) # this will automatically be shared with TIM1_CH1
|
||||||
|
PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) BIDIR
|
||||||
|
PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) BIDIR
|
||||||
|
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 4
|
||||||
|
|
||||||
|
# Pin for PWM Voltage Selection
|
||||||
|
PB4 PWM_VOLT_SEL OUTPUT HIGH GPIO(3)
|
||||||
|
|
||||||
|
# Relays default to use GPIO pins 54 and 55.
|
||||||
|
define RELAY1_PIN_DEFAULT 54
|
||||||
|
define RELAY2_PIN_DEFAULT 55
|
||||||
|
|
||||||
|
# This is the invensense data-ready pin. We don't use it in the
|
||||||
|
# default driver.
|
||||||
|
PD15 MPU_DRDY INPUT
|
||||||
|
|
||||||
|
# the 2nd GPS UART
|
||||||
|
# UART8 serial4 GPS2
|
||||||
|
PE0 UART8_RX UART8
|
||||||
|
PE1 UART8_TX UART8 NODMA
|
||||||
|
|
||||||
|
# Now setup SPI bus4.
|
||||||
|
PE2 SPI4_SCK SPI4
|
||||||
|
PE5 SPI4_MISO SPI4
|
||||||
|
PE6 SPI4_MOSI SPI4
|
||||||
|
|
||||||
|
# This is the pin to enable the sensors rail. It can be used to power
|
||||||
|
# cycle sensors to recover them in case there are problems with power on
|
||||||
|
# timing affecting sensor stability. We pull it LOW on startup, which
|
||||||
|
# means sensors off, then it is pulled HIGH in peripheral_power_enable()
|
||||||
|
PE3 VDD_3V3_SENSORS_EN OUTPUT LOW
|
||||||
|
|
||||||
|
# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters).
|
||||||
|
PE7 UART7_RX UART7
|
||||||
|
PE8 UART7_TX UART7
|
||||||
|
|
||||||
|
# Define a LED, mapping it to GPIO(0). LOW will illuminate the LED
|
||||||
|
PE12 FMU_LED_AMBER OUTPUT HIGH OPENDRAIN GPIO(0)
|
||||||
|
|
||||||
|
# Power flag pins: these tell the MCU the status of the various power
|
||||||
|
# supplies that are available. The pin names need to exactly match the
|
||||||
|
# names used in AnalogIn.cpp.
|
||||||
|
PB5 VDD_BRICK_nVALID INPUT PULLUP
|
||||||
|
PB7 VDD_BRICK2_nVALID INPUT PULLUP
|
||||||
|
PE10 VDD_5V_HIPOWER_nOC INPUT PULLUP
|
||||||
|
PE15 VDD_5V_PERIPH_nOC INPUT PULLUP
|
||||||
|
|
||||||
|
SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ
|
||||||
|
SPIDEV ms5611_ext SPI4 DEVID2 BARO_EXT_CS MODE3 20*MHZ 20*MHZ
|
||||||
|
SPIDEV mpu6000 SPI1 DEVID4 MPU_CS MODE3 2*MHZ 8*MHZ
|
||||||
|
SPIDEV icm20608-am SPI1 DEVID2 ACCEL_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV mpu9250 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV mpu9250_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lsm9ds0_g SPI1 DEVID1 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lsm9ds0_am SPI1 DEVID2 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lsm9ds0_ext_g SPI4 DEVID4 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lsm9ds0_ext_am SPI4 DEVID3 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV icm20602_ext SPI4 DEVID4 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ
|
||||||
|
SPIDEV external0m0 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ
|
||||||
|
SPIDEV external0m1 SPI4 DEVID5 MPU_EXT_CS MODE1 2*MHZ 2*MHZ
|
||||||
|
SPIDEV external0m2 SPI4 DEVID5 MPU_EXT_CS MODE2 2*MHZ 2*MHZ
|
||||||
|
SPIDEV external0m3 SPI4 DEVID5 MPU_EXT_CS MODE3 2*MHZ 2*MHZ
|
||||||
|
SPIDEV pixartPC15 SPI4 DEVID13 ACCEL_EXT_CS MODE3 2*MHZ 2*MHZ
|
||||||
|
|
||||||
|
# three IMUs, but allow for different varients. First two IMUs are
|
||||||
|
# isolated, 3rd isn't
|
||||||
|
IMU Invensense SPI:mpu9250_ext ROTATION_PITCH_180
|
||||||
|
|
||||||
|
# the 3 rotations for the LSM9DS0 driver are for the accel, the gyro
|
||||||
|
# and the H varient of the gyro
|
||||||
|
IMU LSM9DS0 SPI:lsm9ds0_ext_g SPI:lsm9ds0_ext_am ROTATION_ROLL_180_YAW_270 ROTATION_ROLL_180_YAW_90 ROTATION_ROLL_180_YAW_90
|
||||||
|
|
||||||
|
# 3rd non-isolated IMU
|
||||||
|
IMU Invensense SPI:mpu9250 ROTATION_YAW_270
|
||||||
|
|
||||||
|
# alternative IMU set for newer cubes
|
||||||
|
IMU Invensense SPI:icm20602_ext ROTATION_ROLL_180_YAW_270
|
||||||
|
IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180
|
||||||
|
IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270
|
||||||
|
|
||||||
|
# Sensor Check alias for validating board type
|
||||||
|
CHECK_ICM20649 spi_check_register_inv2("icm20948", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649)
|
||||||
|
CHECK_ICM20602_EXT spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)
|
||||||
|
CHECK_ICM20948_EXT spi_check_register_inv2("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948)
|
||||||
|
CHECK_MS5611 check_ms5611("ms5611")
|
||||||
|
CHECK_MS5611_EXT check_ms5611("ms5611_ext")
|
||||||
|
|
||||||
|
# Sensor Check Macros to be used for validating board type
|
||||||
|
CHECK_IMU0_PRESENT $CHECK_ICM20602_EXT
|
||||||
|
CHECK_IMU1_PRESENT $CHECK_ICM20948_EXT
|
||||||
|
CHECK_IMU2_PRESENT $CHECK_ICM20649
|
||||||
|
CHECK_BARO0_PRESENT $CHECK_MS5611
|
||||||
|
CHECK_BARO1_PRESENT $CHECK_MS5611_EXT
|
||||||
|
|
||||||
|
BOARD_VALIDATE $CHECK_IMU0_PRESENT $CHECK_IMU1_PRESENT $CHECK_IMU2_PRESENT $CHECK_BARO0_PRESENT $CHECK_BARO1_PRESENT
|
||||||
|
|
||||||
|
|
||||||
|
define HAL_DEFAULT_INS_FAST_SAMPLE 7
|
||||||
|
|
||||||
|
# two baros
|
||||||
|
BARO MS56XX SPI:ms5611_ext
|
||||||
|
BARO MS56XX SPI:ms5611
|
||||||
|
|
||||||
|
# two compasses. First is in the LSM303D
|
||||||
|
COMPASS LSM303D SPI:lsm9ds0_ext_am ROTATION_YAW_270
|
||||||
|
# 2nd compass is part of the 2nd invensense IMU
|
||||||
|
COMPASS AK8963:probe_mpu9250 1 ROTATION_YAW_270
|
||||||
|
|
||||||
|
# compass as part of ICM20948 on newer cubes
|
||||||
|
COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90
|
||||||
|
|
||||||
|
# also probe for external compasses
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
|
||||||
|
define HAL_CHIBIOS_ARCH_FMUV3 1
|
||||||
|
|
||||||
|
define BOARD_TYPE_DEFAULT 3
|
||||||
|
|
||||||
|
# Nnow some defines for logging and terrain data files.
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
# allow to have have a dedicated safety switch pin
|
||||||
|
define HAL_HAVE_SAFETY_SWITCH 1
|
||||||
|
|
||||||
|
# Enable RAMTROM parameter storage.
|
||||||
|
define HAL_WITH_RAMTRON 1
|
||||||
|
|
||||||
|
# Setup for the possibility of an IMU heater since the pixhawk2 cube has
|
||||||
|
# an IMU heater.
|
||||||
|
define HAL_HAVE_IMU_HEATER 1
|
||||||
|
|
||||||
|
# Enable FAT filesystem support (needs a microSD defined via SDMMC).
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
|
||||||
|
# Now setup the default battery pins driver analog pins and default
|
||||||
|
# scaling for the power brick.
|
||||||
|
define HAL_BATT_VOLT_PIN 14
|
||||||
|
define HAL_BATT_CURR_PIN 15
|
||||||
|
define HAL_BATT_VOLT_SCALE 10.1
|
||||||
|
define HAL_BATT_CURR_SCALE 17.0
|
||||||
|
define HAL_GPIO_PWM_VOLT_PIN 3
|
||||||
|
define HAL_GPIO_PWM_VOLT_3v3 1
|
||||||
|
|
||||||
|
# List of files to put in ROMFS. For fmuv3 we need an IO firmware so
|
||||||
|
# we can automatically update the IOMCU firmware on boot. The format
|
||||||
|
# is "ROMFS ROMFS-filename source-filename". Paths are relative to the
|
||||||
|
# ardupilot root.
|
||||||
|
ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
|
||||||
|
|
||||||
|
DMA_NOSHARE SPI1* SPI4* USART6*
|
||||||
|
|
@ -72,9 +72,12 @@ PC5 RSSI_ADC ADC1
|
|||||||
|
|
||||||
PA2 LED0 OUTPUT LOW
|
PA2 LED0 OUTPUT LOW
|
||||||
|
|
||||||
|
# In order to accommodate bi-directional dshot certain devices cannot be DMA enabled
|
||||||
|
# NODMA indicates these devices, if you remove it they will still not be resolved for DMA
|
||||||
|
|
||||||
# USART1
|
# USART1
|
||||||
PA10 USART1_RX USART1
|
PA10 USART1_RX USART1 NODMA
|
||||||
PA9 USART1_TX USART1
|
PA9 USART1_TX USART1 NODMA
|
||||||
|
|
||||||
# USART2
|
# USART2
|
||||||
PD5 USART2_TX USART2
|
PD5 USART2_TX USART2
|
||||||
@ -88,31 +91,32 @@ PB10 USART3_TX USART3
|
|||||||
PA0 UART4_TX UART4 NODMA
|
PA0 UART4_TX UART4 NODMA
|
||||||
PA1 UART4_RX UART4 NODMA
|
PA1 UART4_RX UART4 NODMA
|
||||||
|
|
||||||
# RC input defaults to timer capture
|
# RC input defaults to UART to allow for bi-dir dshot
|
||||||
PC7 TIM8_CH2 TIM8 RCININT PULLDOWN
|
PC6 USART6_TX USART6
|
||||||
PC6 USART6_TX USART6 NODMA
|
PC7 USART6_RX USART6
|
||||||
|
define HAL_SERIAL6_PROTOCOL SerialProtocol_RCIN
|
||||||
# alt config to allow RCIN on UART for bi-directional
|
define HAL_SERIAL6_BAUD 115
|
||||||
# protocols like FPort
|
|
||||||
PC7 USART6_RX USART6 NODMA ALT(1)
|
|
||||||
|
|
||||||
# UART7, RX only for ESC Telemetry
|
# UART7, RX only for ESC Telemetry
|
||||||
PE7 UART7_RX UART7
|
# No DMA because SPI2 requires the DMA slot
|
||||||
|
PE7 UART7_RX UART7 NODMA
|
||||||
PE8 UART7_TX UART7 NODMA LOW
|
PE8 UART7_TX UART7 NODMA LOW
|
||||||
|
|
||||||
# Motors
|
# Motors, bi-directional dshot capable
|
||||||
PB1 TIM1_CH3N TIM1 PWM(1) GPIO(50)
|
PB1 TIM3_CH4 TIM3 PWM(1) GPIO(50) # M1
|
||||||
PE9 TIM1_CH1 TIM1 PWM(2) GPIO(51)
|
PE9 TIM1_CH1 TIM1 PWM(2) GPIO(51) # M2 CH1 and CH2 can share ICU Channel for BIDIR
|
||||||
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52)
|
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR # M4
|
||||||
PB0 TIM3_CH3 TIM3 PWM(4) GPIO(53)
|
PB0 TIM3_CH3 TIM3 PWM(4) GPIO(53) BIDIR # M3
|
||||||
PC9 TIM3_CH4 TIM3 PWM(5) GPIO(54)
|
# These are not DMA enabled so not dshot capable
|
||||||
PA3 TIM5_CH4 TIM5 PWM(6) GPIO(55)
|
# To get dshot (but not bi-dir dshot) remove the NODMA and add NODMA to USART6
|
||||||
|
PC9 TIM8_CH4 TIM8 PWM(5) GPIO(54) NODMA # M5
|
||||||
|
PA3 TIM5_CH4 TIM5 PWM(6) GPIO(55) # M6
|
||||||
|
|
||||||
# extra PWM outs
|
# extra PWM outs
|
||||||
#PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56) # led pin
|
#PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56) # led pin
|
||||||
#PD15 TIM4_CH4 TIM4 PWM(8) GPIO(57) # buzzer pin (need to comment out buzzer)
|
#PD15 TIM4_CH4 TIM4 PWM(8) GPIO(57) # buzzer pin (need to comment out buzzer)
|
||||||
|
|
||||||
DMA_PRIORITY TIM1* TIM3* SPI4* SPI1*
|
DMA_PRIORITY ADC* USART6* TIM1* TIM3* SPI4* SPI1*
|
||||||
|
|
||||||
define HAL_STORAGE_SIZE 16384
|
define HAL_STORAGE_SIZE 16384
|
||||||
define STORAGE_FLASH_PAGE 1
|
define STORAGE_FLASH_PAGE 1
|
||||||
|
173
libraries/AP_HAL_ChibiOS/hwdef/MatekF405-bdshot/hwdef.dat
Normal file
173
libraries/AP_HAL_ChibiOS/hwdef/MatekF405-bdshot/hwdef.dat
Normal file
@ -0,0 +1,173 @@
|
|||||||
|
# hw definition file for MatekF405 hardware
|
||||||
|
# tested on the MatekF405-OSD board
|
||||||
|
# with thanks to betaflight for pinout
|
||||||
|
|
||||||
|
|
||||||
|
# MCU class and specific type
|
||||||
|
MCU STM32F4xx STM32F405xx
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 125
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 8000000
|
||||||
|
|
||||||
|
define STM32_ST_USE_TIMER 4
|
||||||
|
define CH_CFG_ST_RESOLUTION 16
|
||||||
|
|
||||||
|
FLASH_SIZE_KB 1024
|
||||||
|
|
||||||
|
|
||||||
|
# only one I2C bus
|
||||||
|
I2C_ORDER I2C1
|
||||||
|
|
||||||
|
# order of UARTs (and USB)
|
||||||
|
SERIAL_ORDER OTG1 USART3 UART4 USART1 UART5 USART2
|
||||||
|
|
||||||
|
# LEDs
|
||||||
|
PB9 LED_BLUE OUTPUT LOW GPIO(0)
|
||||||
|
PA14 LED_GREEN OUTPUT LOW GPIO(1)
|
||||||
|
|
||||||
|
define HAL_GPIO_A_LED_PIN 0
|
||||||
|
define HAL_GPIO_B_LED_PIN 1
|
||||||
|
|
||||||
|
# buzzer
|
||||||
|
PC13 BUZZER OUTPUT GPIO(80) LOW
|
||||||
|
define HAL_BUZZER_PIN 80
|
||||||
|
define HAL_BUZZER_ON 1
|
||||||
|
define HAL_BUZZER_OFF 0
|
||||||
|
|
||||||
|
# spi1 bus for IMU
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# spi2 for OSD
|
||||||
|
PB13 SPI2_SCK SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
PB10 MAX7456_CS CS
|
||||||
|
|
||||||
|
# spi3 for sdcard and onboard flash
|
||||||
|
PB3 SPI3_SCK SPI3
|
||||||
|
PB4 SPI3_MISO SPI3
|
||||||
|
PB5 SPI3_MOSI SPI3
|
||||||
|
PC1 SDCARD_CS CS
|
||||||
|
PC0 M25P16_CS CS
|
||||||
|
|
||||||
|
PC2 MPU6000_CS CS
|
||||||
|
|
||||||
|
# only one I2C bus in normal config
|
||||||
|
PB6 I2C1_SCL I2C1
|
||||||
|
PB7 I2C1_SDA I2C1
|
||||||
|
|
||||||
|
# analog pins
|
||||||
|
PC5 BATT_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PC4 BATT_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
PB1 RSSI_ADC_PIN ADC1 SCALE(1)
|
||||||
|
|
||||||
|
# define default battery setup
|
||||||
|
# PC5 - ADC12_CH15
|
||||||
|
define HAL_BATT_VOLT_PIN 15
|
||||||
|
# PC4 - ADC12_CH14
|
||||||
|
define HAL_BATT_CURR_PIN 14
|
||||||
|
define HAL_BATT_VOLT_SCALE 10.1
|
||||||
|
define HAL_BATT_CURR_SCALE 17.0
|
||||||
|
|
||||||
|
#analog rssi pin (also could be used as analog airspeed input)
|
||||||
|
# PB1 - ADC12_CH9
|
||||||
|
define BOARD_RSSI_ANA_PIN 9
|
||||||
|
|
||||||
|
# USART1
|
||||||
|
PA9 USART1_TX USART1
|
||||||
|
PA10 USART1_RX USART1
|
||||||
|
|
||||||
|
# RC input using timer
|
||||||
|
PA3 TIM9_CH2 TIM9 RCININT PULLDOWN
|
||||||
|
|
||||||
|
# alternative RC input using UART
|
||||||
|
PA2 USART2_TX USART2 NODMA
|
||||||
|
PA3 USART2_RX USART2 NODMA ALT(1)
|
||||||
|
|
||||||
|
# USART3
|
||||||
|
PC10 USART3_TX USART3
|
||||||
|
PC11 USART3_RX USART3
|
||||||
|
|
||||||
|
# UART4
|
||||||
|
PA0 UART4_TX UART4
|
||||||
|
PA1 UART4_RX UART4
|
||||||
|
|
||||||
|
# UART5
|
||||||
|
PD2 UART5_RX UART5
|
||||||
|
PC12 UART5_TX UART5
|
||||||
|
|
||||||
|
# PA10 IO-debug-console
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
# USB detection
|
||||||
|
PB12 VBUS INPUT OPENDRAIN
|
||||||
|
|
||||||
|
# debug (disabled out to allow for both LEDs)
|
||||||
|
#PA13 JTMS-SWDIO SWD
|
||||||
|
#PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
# PWM out pins. Note that channel order follows the ArduPilot motor
|
||||||
|
# order conventions
|
||||||
|
PC6 TIM8_CH1 TIM8 PWM(1) GPIO(50) BIDIR
|
||||||
|
PC7 TIM8_CH2 TIM8 PWM(2) GPIO(51)
|
||||||
|
PC8 TIM8_CH3 TIM8 PWM(3) GPIO(52)
|
||||||
|
PC9 TIM8_CH4 TIM8 PWM(4) GPIO(53)
|
||||||
|
PA15 TIM2_CH1 TIM2 PWM(5) GPIO(54)
|
||||||
|
PA8 TIM1_CH1 TIM1 PWM(6) GPIO(55)
|
||||||
|
|
||||||
|
define HAL_STORAGE_SIZE 15360
|
||||||
|
define STORAGE_FLASH_PAGE 2
|
||||||
|
|
||||||
|
# reserve 32k for bootloader and 32k for flash storage
|
||||||
|
FLASH_RESERVE_START_KB 64
|
||||||
|
|
||||||
|
# one IMU
|
||||||
|
IMU Invensense SPI:mpu6000 ROTATION_YAW_180
|
||||||
|
|
||||||
|
# probe for I2C BMP280, but allow init on board variants without onboard baro too
|
||||||
|
BARO BMP280 I2C:0:0x76
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_BAROS
|
||||||
|
define HAL_BARO_ALLOW_INIT_NO_BARO
|
||||||
|
|
||||||
|
# no built-in compass, but probe the i2c bus for all possible
|
||||||
|
# external compass types
|
||||||
|
define ALLOW_ARM_NO_COMPASS
|
||||||
|
define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
define HAL_I2C_INTERNAL_MASK 0
|
||||||
|
define HAL_COMPASS_AUTO_ROT_DEFAULT 2
|
||||||
|
|
||||||
|
# SPI devices
|
||||||
|
SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ
|
||||||
|
SPIDEV sdcard SPI3 DEVID1 SDCARD_CS MODE0 400*KHZ 25*MHZ
|
||||||
|
SPIDEV osd SPI2 DEVID1 MAX7456_CS MODE0 10*MHZ 10*MHZ
|
||||||
|
|
||||||
|
# filesystem setup on sdcard
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
# 8 PWM available by default
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 8
|
||||||
|
|
||||||
|
# setup for OSD
|
||||||
|
define OSD_ENABLED 1
|
||||||
|
define HAL_OSD_TYPE_DEFAULT 1
|
||||||
|
ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
|
||||||
|
|
||||||
|
# disable SMBUS and fuel battery monitors to save flash
|
||||||
|
define HAL_BATTMON_SMBUS_ENABLE 0
|
||||||
|
define HAL_BATTMON_FUEL_ENABLE 0
|
||||||
|
|
||||||
|
# disable parachute and sprayer to save flash
|
||||||
|
define HAL_PARACHUTE_ENABLED 0
|
||||||
|
define HAL_SPRAYER_ENABLED 0
|
||||||
|
|
||||||
|
# reduce max size of embedded params for apj_tool.py
|
||||||
|
define AP_PARAM_MAX_EMBEDDED_PARAM 1024
|
145
libraries/AP_HAL_ChibiOS/hwdef/OMNIBUSF7V2-bdshot/hwdef.dat
Normal file
145
libraries/AP_HAL_ChibiOS/hwdef/OMNIBUSF7V2-bdshot/hwdef.dat
Normal file
@ -0,0 +1,145 @@
|
|||||||
|
# hw definition file for processing by chibios_pins.py
|
||||||
|
# for OMNIBUSF7V2 hardware.
|
||||||
|
@ thanks to betaflight for pin information
|
||||||
|
|
||||||
|
# MCU class and specific type
|
||||||
|
MCU STM32F7xx STM32F745xx
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 121
|
||||||
|
|
||||||
|
# crystal frequency, setup to use external oscillator
|
||||||
|
OSCILLATOR_HZ 8000000
|
||||||
|
|
||||||
|
FLASH_SIZE_KB 1024
|
||||||
|
|
||||||
|
# reserve one sector for bootloader and 2 for storage
|
||||||
|
FLASH_RESERVE_START_KB 96
|
||||||
|
|
||||||
|
|
||||||
|
# only one I2C bus
|
||||||
|
I2C_ORDER I2C2
|
||||||
|
|
||||||
|
# order of UARTs (and USB)
|
||||||
|
SERIAL_ORDER OTG1 USART1 USART3 USART6 USART2 UART7
|
||||||
|
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
PE0 LED0 OUTPUT LOW
|
||||||
|
PD15 TIM4_CH4 TIM4 GPIO(57) ALARM
|
||||||
|
|
||||||
|
# ICM-20608 on SPI1
|
||||||
|
PA4 MPU6000_CS CS
|
||||||
|
|
||||||
|
# MPU6500 on SPI3
|
||||||
|
PA15 MPU6500_CS CS
|
||||||
|
|
||||||
|
# SPI1 for IMU and baro
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# SPI2 for MAX7456 OSD
|
||||||
|
PB12 MAX7456_CS CS
|
||||||
|
PB13 SPI2_SCK SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
|
||||||
|
PC10 SPI3_SCK SPI3
|
||||||
|
PC11 SPI3_MISO SPI3
|
||||||
|
PC12 SPI3_MOSI SPI3
|
||||||
|
|
||||||
|
# SPI4 for SDCard
|
||||||
|
PE4 SDCARD_CS CS
|
||||||
|
PE2 SPI4_SCK SPI4
|
||||||
|
PE5 SPI4_MISO SPI4
|
||||||
|
PE6 SPI4_MOSI SPI4
|
||||||
|
|
||||||
|
# external I2C device2, pins shared with USART3, normally disabled
|
||||||
|
PB10 I2C2_SCL I2C2
|
||||||
|
PB11 I2C2_SDA I2C2
|
||||||
|
|
||||||
|
# baro is BMP280 on SPI1
|
||||||
|
PA1 BMP280_CS CS
|
||||||
|
|
||||||
|
# mag is on I2C if connected
|
||||||
|
|
||||||
|
PC3 BATT_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PC2 BATT_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
PC5 RSSI_ADC ADC1
|
||||||
|
|
||||||
|
# setup two IMUs
|
||||||
|
IMU Invensense SPI:mpu6000 ROTATION_NONE
|
||||||
|
IMU Invensense SPI:mpu6500 ROTATION_YAW_90
|
||||||
|
|
||||||
|
# USART1
|
||||||
|
PA10 USART1_RX USART1
|
||||||
|
PA9 USART1_TX USART1
|
||||||
|
|
||||||
|
# RC input using timer by default
|
||||||
|
PA3 TIM9_CH2 TIM9 RCININT PULLDOWN
|
||||||
|
|
||||||
|
# alternative RC input using USART2
|
||||||
|
PA3 USART2_RX USART2 NODMA ALT(1)
|
||||||
|
|
||||||
|
# USART3, enabled as ALT config in place of I2C
|
||||||
|
PB10 USART3_TX USART3 ALT(2)
|
||||||
|
PB11 USART3_RX USART3 ALT(2)
|
||||||
|
|
||||||
|
# USART6
|
||||||
|
PC7 USART6_RX USART6
|
||||||
|
PC6 USART6_TX USART6
|
||||||
|
|
||||||
|
# UART7 (V2 version, ESC telem)
|
||||||
|
PE7 UART7_RX UART7
|
||||||
|
|
||||||
|
|
||||||
|
PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50)
|
||||||
|
PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) BIDIR
|
||||||
|
PE9 TIM1_CH1 TIM1 PWM(3) GPIO(52) BIDIR
|
||||||
|
PE11 TIM1_CH2 TIM1 PWM(4) GPIO(53)
|
||||||
|
|
||||||
|
DMA_PRIORITY ADC* USART6* TIM1_CH1 TIM3_CH4 TIM1_UP TIM3_UP SPI4* SPI1*
|
||||||
|
|
||||||
|
define HAL_STORAGE_SIZE 16384
|
||||||
|
define STORAGE_FLASH_PAGE 1
|
||||||
|
|
||||||
|
# spi devices
|
||||||
|
SPIDEV bmp280 SPI1 DEVID1 BMP280_CS MODE3 1*MHZ 8*MHZ
|
||||||
|
SPIDEV mpu6000 SPI1 DEVID2 MPU6000_CS MODE3 1*MHZ 4*MHZ
|
||||||
|
SPIDEV mpu6500 SPI3 DEVID1 MPU6500_CS MODE3 1*MHZ 4*MHZ
|
||||||
|
SPIDEV sdcard SPI4 DEVID1 SDCARD_CS MODE0 400*KHZ 25*MHZ
|
||||||
|
SPIDEV osd SPI2 DEVID1 MAX7456_CS MODE0 10*MHZ 10*MHZ
|
||||||
|
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
# no built-in compass, but probe the i2c bus for all possible
|
||||||
|
# external compass types
|
||||||
|
define ALLOW_ARM_NO_COMPASS
|
||||||
|
define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
define HAL_I2C_INTERNAL_MASK 0
|
||||||
|
define HAL_COMPASS_AUTO_ROT_DEFAULT 2
|
||||||
|
|
||||||
|
# disable SMBUS and fuel battery monitors to save flash
|
||||||
|
define HAL_BATTMON_SMBUS_ENABLE 0
|
||||||
|
define HAL_BATTMON_FUEL_ENABLE 0
|
||||||
|
|
||||||
|
# disable parachute and sprayer to save flash
|
||||||
|
define HAL_PARACHUTE_ENABLED 0
|
||||||
|
define HAL_SPRAYER_ENABLED 0
|
||||||
|
|
||||||
|
# one baro
|
||||||
|
BARO BMP280 SPI:bmp280
|
||||||
|
|
||||||
|
define OSD_ENABLED 1
|
||||||
|
define OSD_PARAM_ENABLED 0
|
||||||
|
define HAL_OSD_TYPE_DEFAULT 1
|
||||||
|
ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
|
150
libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6-bdshot/hwdef.dat
Normal file
150
libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6-bdshot/hwdef.dat
Normal file
@ -0,0 +1,150 @@
|
|||||||
|
# hw definition file for processing by chibios_pins.py
|
||||||
|
# Omnibus F4 Nano V6 only
|
||||||
|
# with F405 mcu, mpu6000 imu, bmp280 barometer, 7456 series osd, flash log storage
|
||||||
|
|
||||||
|
MCU STM32F4xx STM32F405xx
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 133
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 8000000
|
||||||
|
|
||||||
|
STM32_ST_USE_TIMER 5
|
||||||
|
|
||||||
|
# flash size
|
||||||
|
FLASH_SIZE_KB 1024
|
||||||
|
FLASH_RESERVE_START_KB 64
|
||||||
|
|
||||||
|
# order of I2C buses
|
||||||
|
I2C_ORDER I2C2 I2C1
|
||||||
|
|
||||||
|
# order of UARTs
|
||||||
|
SERIAL_ORDER OTG1 USART1 UART4 USART6 USART3
|
||||||
|
|
||||||
|
#adc
|
||||||
|
PC1 BAT_CURR_SENS ADC1 SCALE(1)
|
||||||
|
PC2 BAT_VOLT_SENS ADC1 SCALE(1)
|
||||||
|
PA0 RSSI_IN ADC1
|
||||||
|
|
||||||
|
#pwm output
|
||||||
|
PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50)
|
||||||
|
PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) BIDIR
|
||||||
|
PA3 TIM2_CH4 TIM2 PWM(3) GPIO(52) BIDIR
|
||||||
|
PB5 TIM3_CH2 TIM3 PWM(4) GPIO(53) BIDIR
|
||||||
|
|
||||||
|
PA4 MPU6000_CS CS
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# note that this board needs PULLUP on I2C pins
|
||||||
|
PB10 I2C2_SCL I2C2 PULLUP
|
||||||
|
PB11 I2C2_SDA I2C2 PULLUP
|
||||||
|
|
||||||
|
# I2C1 on PPM / PB9 pads of V6.x board revision
|
||||||
|
PB8 I2C1_SCL I2C1 PULLUP
|
||||||
|
PB9 I2C1_SDA I2C1 PULLUP
|
||||||
|
|
||||||
|
# use RX3 / TX3 pins as USART3 = SERIAL4 in BRD_ALT_CONFIG = 1
|
||||||
|
PB10 USART3_TX USART3 ALT(1)
|
||||||
|
PB11 USART3_RX USART3 ALT(1)
|
||||||
|
|
||||||
|
# SPI2 for flash
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB13 SPI2_SCK SPI2
|
||||||
|
PB12 FLASH_CS CS
|
||||||
|
|
||||||
|
# USART1 = SERIAL1 on original V6 revision only
|
||||||
|
# not available on V6.x revisions due to altered inverter layout
|
||||||
|
PA10 USART1_RX USART1
|
||||||
|
PA9 USART1_TX USART1
|
||||||
|
|
||||||
|
# USART6 = SERIAL3
|
||||||
|
PC6 USART6_TX USART6
|
||||||
|
PC7 USART6_RX USART6
|
||||||
|
|
||||||
|
# UART4 (ESC sensor)
|
||||||
|
PA1 UART4_RX UART4
|
||||||
|
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
PA15 OSD_CS CS
|
||||||
|
PB3 BMP280_CS CS
|
||||||
|
PC12 SPI3_MOSI SPI3
|
||||||
|
PC11 SPI3_MISO SPI3
|
||||||
|
PC10 SPI3_SCK SPI3
|
||||||
|
|
||||||
|
PA8 LED OUTPUT HIGH GPIO(41)
|
||||||
|
|
||||||
|
# passive buzzer disabled, timer 3 used for PWM(4) output
|
||||||
|
#PB4 TIM3_CH1 TIM3 GPIO(70) ALARM
|
||||||
|
|
||||||
|
# use active buzzer instead
|
||||||
|
PB4 BUZZER OUTPUT GPIO(80) LOW
|
||||||
|
define HAL_BUZZER_PIN 80
|
||||||
|
define HAL_BUZZER_ON 1
|
||||||
|
define HAL_BUZZER_OFF 0
|
||||||
|
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
PC5 VBUS INPUT OPENDRAIN
|
||||||
|
|
||||||
|
#LED strip output pad used for RC input
|
||||||
|
PB6 TIM4_CH1 TIM4 RCININT PULLDOWN LOW
|
||||||
|
|
||||||
|
#Omnibus F4 V3 and later had hw inverter on UART6
|
||||||
|
#Overide it to use as GPS UART port
|
||||||
|
PC8 SBUS_INVERT_RX OUTPUT LOW
|
||||||
|
PC9 SBUS_INVERT_TX OUTPUT LOW
|
||||||
|
|
||||||
|
# SPI Device table
|
||||||
|
SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ
|
||||||
|
SPIDEV bmp280 SPI3 DEVID3 BMP280_CS MODE3 1*MHZ 8*MHZ
|
||||||
|
SPIDEV osd SPI3 DEVID4 OSD_CS MODE0 10*MHZ 10*MHZ
|
||||||
|
SPIDEV dataflash SPI2 DEVID1 FLASH_CS MODE3 32*MHZ 32*MHZ
|
||||||
|
|
||||||
|
# enable logging to dataflash
|
||||||
|
define HAL_LOGGING_DATAFLASH
|
||||||
|
|
||||||
|
# one IMU
|
||||||
|
IMU Invensense SPI:mpu6000 ROTATION_YAW_90
|
||||||
|
|
||||||
|
# one baro
|
||||||
|
BARO BMP280 SPI:bmp280
|
||||||
|
|
||||||
|
# no built-in compass, but probe the i2c bus for all possible
|
||||||
|
# external compass types
|
||||||
|
define ALLOW_ARM_NO_COMPASS
|
||||||
|
define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
define HAL_I2C_INTERNAL_MASK 0
|
||||||
|
|
||||||
|
define STORAGE_FLASH_PAGE 1
|
||||||
|
define HAL_STORAGE_SIZE 15360
|
||||||
|
|
||||||
|
|
||||||
|
# define default battery setup
|
||||||
|
define HAL_BATT_VOLT_PIN 12
|
||||||
|
define HAL_BATT_CURR_PIN 11
|
||||||
|
define HAL_BATT_VOLT_SCALE 11
|
||||||
|
define HAL_BATT_CURR_SCALE 18.2
|
||||||
|
|
||||||
|
#analog rssi pin (also could be used as analog airspeed input)
|
||||||
|
#PA0 - ADC123_CH0
|
||||||
|
define BOARD_RSSI_ANA_PIN 0
|
||||||
|
|
||||||
|
|
||||||
|
define HAL_GPIO_A_LED_PIN 41
|
||||||
|
|
||||||
|
define OSD_ENABLED 1
|
||||||
|
define HAL_OSD_TYPE_DEFAULT 1
|
||||||
|
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 4
|
||||||
|
|
||||||
|
#font for the osd
|
||||||
|
ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
|
||||||
|
|
||||||
|
define HAL_MOUNT_ENABLED 0
|
40
libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M-bdshot/hwdef.dat
Normal file
40
libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M-bdshot/hwdef.dat
Normal file
@ -0,0 +1,40 @@
|
|||||||
|
# hw definition file for processing by chibios_hwdef.py
|
||||||
|
# for Pixhawk1-1M, based on Pixhawk1
|
||||||
|
|
||||||
|
|
||||||
|
include ../fmuv3-bdshot/hwdef.dat
|
||||||
|
|
||||||
|
# define the IMU types to probe. You can list more IMUs than you
|
||||||
|
# actually have. The syntax for IMU declarations is:
|
||||||
|
# IMU DriverName DeviceDeclarations Arguments
|
||||||
|
# the DriverName is the C++ name of the driver in AP_InertialSensor
|
||||||
|
# the DeviceDeclarations are either SPI or I2C
|
||||||
|
# for SPI the format is SPI:devicename where devicename comes from the
|
||||||
|
# SPIDEV table above
|
||||||
|
# for I2C the format is I2C:BusNum:BusAddr7bit, for example I2C:0:0x1e
|
||||||
|
#
|
||||||
|
# for fmuv3 we normally have two IMUs, one is a mpu6000 with a single SPI CS
|
||||||
|
# line. The other is a LSM303D/L3GD20 which uses the LSM9DS0 driver,
|
||||||
|
# and two SPIDEV endpoints, one for gyro, one for accel/mag
|
||||||
|
|
||||||
|
IMU Invensense SPI:mpu6000 ROTATION_ROLL_180
|
||||||
|
IMU LSM9DS0 SPI:lsm9ds0_g SPI:lsm9ds0_am ROTATION_ROLL_180 ROTATION_ROLL_180_YAW_270 ROTATION_PITCH_180
|
||||||
|
|
||||||
|
# define the barometers to probe with BARO lines. These follow the
|
||||||
|
# same format as IMU lines
|
||||||
|
BARO MS56XX SPI:ms5611
|
||||||
|
|
||||||
|
# probe for two mags.
|
||||||
|
# note that the number of arguments and meanings for compass driver
|
||||||
|
# declarations is driver dependent. The HMC5843 driver takes two
|
||||||
|
# arguments, the first for whether the compass is external and the
|
||||||
|
# second the orientation. The LSM303D driver doesn't take any arguments
|
||||||
|
COMPASS HMC5843 SPI:hmc5843 false ROTATION_PITCH_180
|
||||||
|
COMPASS LSM303D SPI:lsm9ds0_am ROTATION_NONE
|
||||||
|
|
||||||
|
# also probe for external compasses
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
|
||||||
|
FLASH_SIZE_KB 1024
|
||||||
|
define HAL_MINIMIZE_FEATURES 1
|
||||||
|
undef STORAGE_FLASH_PAGE
|
11
libraries/AP_HAL_ChibiOS/hwdef/Pixhawk4-bdshot/hwdef.dat
Normal file
11
libraries/AP_HAL_ChibiOS/hwdef/Pixhawk4-bdshot/hwdef.dat
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
# hw definition file for processing by chibios_hwdef.py
|
||||||
|
# for Holybro Pixhawk4 hardware.
|
||||||
|
|
||||||
|
include ../fmuv5-bdshot/hwdef.dat
|
||||||
|
|
||||||
|
# setup for supplied power brick
|
||||||
|
undef HAL_BATT_VOLT_SCALE
|
||||||
|
define HAL_BATT_VOLT_SCALE 18.182
|
||||||
|
undef HAL_BATT_CURR_SCALE
|
||||||
|
define HAL_BATT_CURR_SCALE 36.364
|
||||||
|
|
234
libraries/AP_HAL_ChibiOS/hwdef/Pixracer-bdshot/hwdef.dat
Normal file
234
libraries/AP_HAL_ChibiOS/hwdef/Pixracer-bdshot/hwdef.dat
Normal file
@ -0,0 +1,234 @@
|
|||||||
|
# hw definition file for processing by chibios_hwdef.py for the
|
||||||
|
# mRo Pixracer board. This is a fmuv4 board
|
||||||
|
|
||||||
|
define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_CHIBIOS_FMUV4
|
||||||
|
|
||||||
|
# MCU class and specific type
|
||||||
|
MCU STM32F4xx STM32F427xx
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 11
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 24000000
|
||||||
|
|
||||||
|
|
||||||
|
# ChibiOS system timer
|
||||||
|
STM32_ST_USE_TIMER 5
|
||||||
|
|
||||||
|
# flash size
|
||||||
|
FLASH_SIZE_KB 2048
|
||||||
|
|
||||||
|
env OPTIMIZE -O2
|
||||||
|
|
||||||
|
# serial port for stdout disabled, use USB console
|
||||||
|
# STDOUT_SERIAL SD7
|
||||||
|
# STDOUT_BAUDRATE 57600
|
||||||
|
|
||||||
|
# only one I2C bus
|
||||||
|
I2C_ORDER I2C1
|
||||||
|
|
||||||
|
# to match px4 we make the first bus number 1
|
||||||
|
define HAL_I2C_BUS_BASE 1
|
||||||
|
define HAL_I2C_INTERNAL_MASK 0
|
||||||
|
|
||||||
|
# order of UARTs (and USB)
|
||||||
|
SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 USART1 UART7
|
||||||
|
|
||||||
|
# UART4 is GPS
|
||||||
|
PA0 UART4_TX UART4
|
||||||
|
PA1 UART4_RX UART4
|
||||||
|
PA2 BATT_VOLTAGE_SENS ADC1
|
||||||
|
PA3 BATT_CURRENT_SENS ADC1
|
||||||
|
PA4 VDD_5V_SENS ADC1 SCALE(2)
|
||||||
|
|
||||||
|
# SPI1 is sensors bus
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
PA9 VBUS INPUT
|
||||||
|
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
# PWM output for buzzer
|
||||||
|
PA15 TIM2_CH1 TIM2 GPIO(77) ALARM
|
||||||
|
|
||||||
|
# control of spektrum power pin
|
||||||
|
PE4 SPEKTRUM_PWR OUTPUT LOW GPIO(70)
|
||||||
|
define HAL_GPIO_SPEKTRUM_PWR 70
|
||||||
|
|
||||||
|
# spektrum power is active low
|
||||||
|
define HAL_SPEKTRUM_PWR_ENABLED 0
|
||||||
|
|
||||||
|
# spektrum RC input pin, used as GPIO for bind for satellite receivers
|
||||||
|
PB0 SPEKTRUM_RC INPUT PULLUP GPIO(71)
|
||||||
|
define HAL_GPIO_SPEKTRUM_RC 71
|
||||||
|
|
||||||
|
PB2 BOOT1 INPUT
|
||||||
|
PB5 VDD_BRICK_VALID INPUT PULLDOWN
|
||||||
|
|
||||||
|
# USART1 is ESP8266
|
||||||
|
PB6 USART1_TX USART1 NODMA
|
||||||
|
PB7 USART1_RX USART1 NODMA
|
||||||
|
PA8 USART1_RTS USART1
|
||||||
|
# PE10 is not a hw CTS pin for USART1
|
||||||
|
PE10 8266_CTS INPUT
|
||||||
|
|
||||||
|
# make GPIOs for ESP8266 available via mavlink relay control as pins
|
||||||
|
# 60 to 63
|
||||||
|
PB4 8266_GPIO2 OUTPUT GPIO(60)
|
||||||
|
PE2 8266_GPI0 INPUT PULLUP GPIO(61)
|
||||||
|
PE5 8266_PD OUTPUT HIGH GPIO(62)
|
||||||
|
PE6 8266_RST OUTPUT HIGH GPIO(63)
|
||||||
|
|
||||||
|
PB8 I2C1_SCL I2C1
|
||||||
|
PB9 I2C1_SDA I2C1
|
||||||
|
|
||||||
|
# SPI2 is FRAM
|
||||||
|
PB10 SPI2_SCK SPI2
|
||||||
|
PB12 CAN2_RX CAN2
|
||||||
|
PB13 CAN2_TX CAN2 # this is SPI2_SCK on beta board
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
|
||||||
|
PC0 VBUS_VALID INPUT PULLDOWN
|
||||||
|
PC1 RSSI_IN ADC1
|
||||||
|
PC2 MPU9250_CS CS
|
||||||
|
PC3 LED_SAFETY OUTPUT
|
||||||
|
PC4 SAFETY_IN INPUT PULLDOWN
|
||||||
|
PC5 VDD_PERIPH_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
PC7 TIM8_CH2 TIM8 RCININT FLOAT LOW # also USART6_RX for serial RC
|
||||||
|
PC13 SBUS_INV OUTPUT LOW
|
||||||
|
|
||||||
|
PC8 SDIO_D0 SDIO
|
||||||
|
PC9 SDIO_D1 SDIO
|
||||||
|
PC10 SDIO_D2 SDIO
|
||||||
|
PC11 SDIO_D3 SDIO
|
||||||
|
PC12 SDIO_CK SDIO
|
||||||
|
|
||||||
|
|
||||||
|
PC14 20608_DRDY INPUT
|
||||||
|
PC15 20608_CS CS
|
||||||
|
|
||||||
|
PD0 CAN1_RX CAN1
|
||||||
|
PD1 CAN1_TX CAN1
|
||||||
|
PD2 SDIO_CMD SDIO
|
||||||
|
|
||||||
|
# USART2 serial2 telem1
|
||||||
|
PD3 USART2_CTS USART2
|
||||||
|
PD4 USART2_RTS USART2
|
||||||
|
PD5 USART2_TX USART2
|
||||||
|
PD6 USART2_RX USART2
|
||||||
|
|
||||||
|
PD7 BARO_CS CS
|
||||||
|
|
||||||
|
# USART3 serial3 telem2
|
||||||
|
PD8 USART3_TX USART3
|
||||||
|
PD9 USART3_RX USART3
|
||||||
|
PD10 FRAM_CS CS
|
||||||
|
PD11 USART3_CTS USART3
|
||||||
|
PD12 USART3_RTS USART3
|
||||||
|
|
||||||
|
PD15 MPU9250_DRDY INPUT
|
||||||
|
|
||||||
|
# UART8 serial4 FrSky
|
||||||
|
PE0 UART8_RX UART8
|
||||||
|
PE1 UART8_TX UART8
|
||||||
|
|
||||||
|
# allow this uart to be inverted for transmit under user control
|
||||||
|
# the polarity is the value to use on the GPIO to change the polarity
|
||||||
|
# to the opposite of the default
|
||||||
|
PA10 UART8_TXINV OUTPUT HIGH GPIO(78) POL(0)
|
||||||
|
|
||||||
|
PE3 VDD_SENSORS_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# UART7 is debug
|
||||||
|
PE7 UART7_RX UART7 NODMA
|
||||||
|
PE8 UART7_TX UART7 NODMA
|
||||||
|
|
||||||
|
PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) BIDIR
|
||||||
|
PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51)
|
||||||
|
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52)
|
||||||
|
PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) BIDIR
|
||||||
|
PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54)
|
||||||
|
PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55)
|
||||||
|
|
||||||
|
DMA_PRIORITY SDIO TIM1_UP TIM1_CH4 TIM1_CH1
|
||||||
|
DMA_NOSHARE SPI1*
|
||||||
|
|
||||||
|
PE12 MAG_DRDY INPUT
|
||||||
|
PE15 MAG_CS CS
|
||||||
|
|
||||||
|
# SPI device table. The DEVID values are chosen to match the PX4 port
|
||||||
|
# of ArduPilot so users don't need to re-do their accel and compass calibrations
|
||||||
|
# when moving to ChibiOS
|
||||||
|
SPIDEV ms5611_int SPI2 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ
|
||||||
|
SPIDEV mpu9250 SPI1 DEVID4 MPU9250_CS MODE3 2*MHZ 4*MHZ
|
||||||
|
SPIDEV icm20608 SPI1 DEVID6 20608_CS MODE3 2*MHZ 8*MHZ
|
||||||
|
SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lis3mdl SPI1 DEVID5 MAG_CS MODE3 500*KHZ 500*KHZ
|
||||||
|
SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ
|
||||||
|
|
||||||
|
define HAL_CHIBIOS_ARCH_FMUV4 1
|
||||||
|
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
define HAL_STORAGE_SIZE 16384
|
||||||
|
|
||||||
|
# enable RAMTROM parameter storage
|
||||||
|
define HAL_WITH_RAMTRON 1
|
||||||
|
|
||||||
|
# enable FAT filesystem
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
|
||||||
|
# pixracer has 3 LEDs, Red, Green, Blue
|
||||||
|
define HAL_HAVE_PIXRACER_LED
|
||||||
|
|
||||||
|
define HAL_GPIO_LED_ON 0
|
||||||
|
define HAL_GPIO_LED_OFF 1
|
||||||
|
|
||||||
|
# LED setup for PixracerLED driver
|
||||||
|
PB11 LED_RED OUTPUT GPIO(0)
|
||||||
|
PB1 LED_GREEN OUTPUT GPIO(1)
|
||||||
|
PB3 LED_BLUE OUTPUT GPIO(2)
|
||||||
|
|
||||||
|
define HAL_GPIO_A_LED_PIN 0
|
||||||
|
define HAL_GPIO_B_LED_PIN 1
|
||||||
|
define HAL_GPIO_C_LED_PIN 2
|
||||||
|
|
||||||
|
# battery setup
|
||||||
|
define HAL_BATT_VOLT_PIN 2
|
||||||
|
define HAL_BATT_CURR_PIN 3
|
||||||
|
define HAL_BATT_VOLT_SCALE 10.1
|
||||||
|
define HAL_BATT_CURR_SCALE 17.0
|
||||||
|
|
||||||
|
# setup serial port defaults for ESP8266
|
||||||
|
define HAL_SERIAL5_PROTOCOL SerialProtocol_MAVLink2
|
||||||
|
define HAL_SERIAL5_BAUD 921600
|
||||||
|
|
||||||
|
# 6 PWM available by default
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 6
|
||||||
|
|
||||||
|
|
||||||
|
# two IMUs
|
||||||
|
IMU Invensense SPI:icm20608 ROTATION_ROLL_180_YAW_90
|
||||||
|
IMU Invensense SPI:mpu9250 ROTATION_ROLL_180_YAW_90
|
||||||
|
define HAL_DEFAULT_INS_FAST_SAMPLE 1
|
||||||
|
|
||||||
|
# 2 compasses. R15 has LIS3MDL instead of HMC5843
|
||||||
|
COMPASS HMC5843 SPI:hmc5843 false ROTATION_PITCH_180
|
||||||
|
COMPASS LIS3MDL SPI:lis3mdl false ROTATION_NONE
|
||||||
|
COMPASS AK8963:probe_mpu9250 0 ROTATION_ROLL_180_YAW_90
|
||||||
|
|
||||||
|
# also probe all types of external I2C compasses
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
|
||||||
|
# one barometer
|
||||||
|
BARO MS56XX SPI:ms5611_int
|
477
libraries/AP_HAL_ChibiOS/hwdef/fmuv3-bdshot/hwdef.dat
Normal file
477
libraries/AP_HAL_ChibiOS/hwdef/fmuv3-bdshot/hwdef.dat
Normal file
@ -0,0 +1,477 @@
|
|||||||
|
# hw definition file for processing by chibios_hwdef.py
|
||||||
|
# for FMUv3 hardware (ie. for Pixhawk1, Pixhawk2 cube, XUAV2.1 etc)
|
||||||
|
|
||||||
|
# This hwdef.dat file contains a lot of comments so it can act as a
|
||||||
|
# reference for developers adding new boards.
|
||||||
|
|
||||||
|
# The hwdef.dat file defines all the hardware peripherals and pins for
|
||||||
|
# a port of ArduPilot to a board using the ChibiOS HAL. You should be
|
||||||
|
# able to write the hwdef.dat file for a new board with just the
|
||||||
|
# schematic for the board.
|
||||||
|
|
||||||
|
# This file is processed by chibios_hwdef.py to create hwdef.h for
|
||||||
|
# this board. You may find it useful to run chibios_hwdef.py manually
|
||||||
|
# when building this file for a new board. The resulting hwdef.h file
|
||||||
|
# is formatted to make it quite readable. It is strongly suggested
|
||||||
|
# that you read the resulting hwdef.h file when porting to a new board
|
||||||
|
# to make sure it has resulted in what you want.
|
||||||
|
|
||||||
|
# You should read this file in conjunction with the schematic for your
|
||||||
|
# board, the datasheet for the MCU for your board and the python
|
||||||
|
# tables file that we have extracted from the datasheet for your
|
||||||
|
# MCU. The python tables file is particularly important, so if you
|
||||||
|
# haven't seen it before go and look at it now. For the STM32F427 it
|
||||||
|
# it called STM32F427xx.py and it is in the hwdef/script/ directory
|
||||||
|
# inside the HAL_ChibiOS directory. That file tells you what each pin
|
||||||
|
# can do (the alternate functions table) and what DMA channels can be
|
||||||
|
# used for each peripheral type. The alternative functions table is
|
||||||
|
# particularly useful when doing a new hwdef.dat file as you can work
|
||||||
|
# out peripheral numbers given a port/pin name.
|
||||||
|
|
||||||
|
# We need to start off by saying what main CPU is on the board. There
|
||||||
|
# are two CPU identifiers that you need to specify. The first is the
|
||||||
|
# ChibiOS MCU type. So far we only support STM32F4xx for all STM32F4
|
||||||
|
# board types. In the future we will add F7 and other MCU types
|
||||||
|
# The second string needs to match the name of a config file in the
|
||||||
|
# libraries/AP_HAL_ChibiOS/hwdef/script directory. In this case we are
|
||||||
|
# using a F427 MCU, so we select STM32F427xx to match the
|
||||||
|
# STM32F427xx.py file in the script directory. If you are supporting a
|
||||||
|
# board type that doesn't have a python hardware database file yet
|
||||||
|
# then you will need to create one. There are scripts in the scripts
|
||||||
|
# directory to help with that by parsing the STM32 datasheets to
|
||||||
|
# extract the required DMA and alternate function tables.
|
||||||
|
|
||||||
|
# MCU class and specific type
|
||||||
|
MCU STM32F4xx STM32F427xx
|
||||||
|
|
||||||
|
# We set a specific HAL_BOARD_SUBTYPE, allowing for custom config in
|
||||||
|
# drivers. For this to be used the subtype needs to be added to
|
||||||
|
# AP_HAL/AP_HAL_Boards.h as well.
|
||||||
|
define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3
|
||||||
|
|
||||||
|
# Now we need to specify the APJ_BOARD_ID. This is the ID that the
|
||||||
|
# bootloader presents to GCS software so it knows if this firmware is
|
||||||
|
# suitable for the board. Please see
|
||||||
|
# https://github.com/ArduPilot/Bootloader/blob/master/hw_config.h for
|
||||||
|
# a list of current board IDs. If you add a new board type then please
|
||||||
|
# get it added to that repository so we don't get conflicts.
|
||||||
|
|
||||||
|
# Note that APJ is "ArduPilot JSON Firmware Format".
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 9
|
||||||
|
|
||||||
|
# Now you need to say what crystal frequency you have for this
|
||||||
|
# board. All of the clocks are scaled against this. Typical values are
|
||||||
|
# 24000000 or 8000000.
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 24000000
|
||||||
|
|
||||||
|
# On some boards you will need to also set the various PLL values. See
|
||||||
|
# the defaults in common/mcuconf.h, and use the define mechanism
|
||||||
|
# explained later in this file to override values suitable for your
|
||||||
|
# board. Refer to your MCU datasheet or examples from supported boards
|
||||||
|
# in ChibiOS for the right values.
|
||||||
|
|
||||||
|
# Now define the voltage the MCU runs at. This is needed for ChibiOS
|
||||||
|
# to set various internal driver limits. It is in 0.01 volt units.
|
||||||
|
|
||||||
|
|
||||||
|
# This is the STM32 timer that ChibiOS will use for the low level
|
||||||
|
# driver. This must be a 32 bit timer. We currently only support
|
||||||
|
# timers 2, 3, 4, 5 and 21. See hal_st_lld.c in ChibiOS for details.
|
||||||
|
|
||||||
|
# ChibiOS system timer
|
||||||
|
STM32_ST_USE_TIMER 5
|
||||||
|
|
||||||
|
# Now the size of flash in kilobytes, for creating the ld.script.
|
||||||
|
|
||||||
|
# flash size
|
||||||
|
FLASH_SIZE_KB 2048
|
||||||
|
|
||||||
|
# Now define which UART is used for printf(). We rarely use printf()
|
||||||
|
# in ChibiOS, so this is really only for debugging very early startup
|
||||||
|
# in drivers.
|
||||||
|
|
||||||
|
# Serial port for stdout. This is optional. If you leave it out then
|
||||||
|
# output from printf() lines will go to the ArduPilot console, which is the
|
||||||
|
# first UART in the SERIAL_ORDER list. But note that some startup code
|
||||||
|
# runs before USB is set up.
|
||||||
|
# The value for STDOUT_SERIAL is a serial device name, and must be for a
|
||||||
|
# serial device for which pins are defined in this file. For example, SD7
|
||||||
|
# is for UART7 (SD7 == "serial device 7" in ChibiOS).
|
||||||
|
#STDOUT_SERIAL SD7
|
||||||
|
#STDOUT_BAUDRATE 57600
|
||||||
|
|
||||||
|
# Now the USB setup, if you have USB. All of these settings are
|
||||||
|
# option, and the ones below are the defaults. It ends up creating a
|
||||||
|
# USB ID on Linux like this:
|
||||||
|
# /dev/serial/by-id/usb-ArduPilot_fmuv3_3E0031000B51353233343932-if00
|
||||||
|
# If creating a board for a RTF vehicle you may wish to customise these.
|
||||||
|
|
||||||
|
# USB setup
|
||||||
|
USB_STRING_MANUFACTURER "ArduPilot"
|
||||||
|
USB_STRING_PRODUCT "%BOARD%"
|
||||||
|
|
||||||
|
# Now define the order that I2C buses are presented in the hal.i2c API
|
||||||
|
# in ArduPilot. For historical reasons inherited from HAL_PX4 the
|
||||||
|
# 'external' I2C bus should be bus 1 in hal.i2c, and internal I2C bus
|
||||||
|
# should be bus 0. On fmuv3 the STM32 I2C1 is our external bus and
|
||||||
|
# I2C2 is our internal bus, so we need to setup the order as I2C2
|
||||||
|
# followed by I2C1 in order to achieve the conventional order that
|
||||||
|
# drivers expect.
|
||||||
|
|
||||||
|
# order of I2C buses
|
||||||
|
I2C_ORDER I2C2 I2C1
|
||||||
|
|
||||||
|
# Now the serial ordering. These map to the SERIALn_ parameter numbers
|
||||||
|
# If you use a shorter list then HAL_Empty::UARTDriver
|
||||||
|
# objects are substituted for later UARTs, or you can leave a gap by
|
||||||
|
# listing one or more of the uarts as EMPTY.
|
||||||
|
|
||||||
|
# The normal usage of this ordering is:
|
||||||
|
# 1) SERIAL0: console (primary mavlink, usually USB)
|
||||||
|
# 2) SERIAL1: telem1
|
||||||
|
# 3) SERIAL2: telem2
|
||||||
|
# 4) SERIAL3: primary GPS
|
||||||
|
# 5) SERIAL4: GPS2
|
||||||
|
# 6) SERIAL5: extra UART (usually RTOS debug console)
|
||||||
|
|
||||||
|
# order of UARTs (and USB)
|
||||||
|
SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 UART7
|
||||||
|
|
||||||
|
# If the board has an IOMCU connected via a UART then this defines the
|
||||||
|
# UART to talk to that MCU. Leave it out for boards with no IOMCU.
|
||||||
|
|
||||||
|
# UART for IOMCU
|
||||||
|
IOMCU_UART USART6
|
||||||
|
|
||||||
|
# Now we start on the pin definitions. Every pin used by ArduPilot
|
||||||
|
# needs to be in this file. The pins in this file can be defined in any order.
|
||||||
|
|
||||||
|
# The format is P+port+pin. So PC4 is portC pin4.
|
||||||
|
# For every pin the second column is the label. If this is a
|
||||||
|
# peripheral that has an alternate function defined in the STM32
|
||||||
|
# datasheet then the label must be the name of that alternative
|
||||||
|
# function. The names are looked up in the python database for this
|
||||||
|
# MCU. Please see STM32F427xx.py for the F427 database. That database
|
||||||
|
# is used to automatically fill in the alternative function (and later
|
||||||
|
# for the DMA channels).
|
||||||
|
|
||||||
|
# The third column is the peripheral type. This must be one of the
|
||||||
|
# following: UARTn, USARTn, OTGn, SPIn, I2Cn, ADCn, TIMn, SWD, SDIO,
|
||||||
|
# INPUT, OUTPUT, CS.
|
||||||
|
|
||||||
|
# The fourth and later columns are for modifiers on the pin. The
|
||||||
|
# possible modifiers are:
|
||||||
|
# pin speed: SPEED_VERYLOW, SPEED_LOW, SPEED_MEDIUM, SPEED_HIGH
|
||||||
|
# pullup: PULLUP, PULLDOWN, FLOATING
|
||||||
|
# out type: OPENDRAIN, PUSHPULL
|
||||||
|
# default value: LOW, HIGH
|
||||||
|
|
||||||
|
# Additionally, each class of pin peripheral can have extra modifiers
|
||||||
|
# suitable for that pin type. For example, for an OUTPUT you can map
|
||||||
|
# it to a GPIO number in hal.gpio using the GPIO(n) modifier. For ADC
|
||||||
|
# inputs you can apply a scaling factor (to bring it to unit volts)
|
||||||
|
# using the SCALE(x) modifier. See the examples below for more
|
||||||
|
# modifiers, or read the python code in chibios_hwdef.py.
|
||||||
|
|
||||||
|
# Now we define UART4 which is for the GPS. Be careful
|
||||||
|
# of the difference between USART and UART. Check the STM32F427xx.py
|
||||||
|
# if unsure which it is. For a UART we need to specify at least TX and
|
||||||
|
# RX pins.
|
||||||
|
|
||||||
|
# UART4 serial GPS
|
||||||
|
PA0 UART4_TX UART4
|
||||||
|
PA1 UART4_RX UART4
|
||||||
|
|
||||||
|
# Now define the primary battery connectors. The labels we choose here
|
||||||
|
# are used to create defines for pins in the various drivers, so
|
||||||
|
# choose names that match existing board setups where possible. Here
|
||||||
|
# we define two pins PA2 and PA3 for voltage and current sensing, with
|
||||||
|
# a scale factor of 1.0 and connected on ADC1. The pin number this
|
||||||
|
# maps to in hal.adc is automatically determined using the datasheet
|
||||||
|
# tables in STM32F427xx.py.
|
||||||
|
|
||||||
|
PA2 BATT_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PA3 BATT_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
# Now the VDD sense pin. This is used to sense primary board voltage.
|
||||||
|
PA4 VDD_5V_SENS ADC1 SCALE(2)
|
||||||
|
|
||||||
|
# Now the first SPI bus. At minimum you need SCK, MISO and MOSI pin
|
||||||
|
definitions. You can add speed modifiers if you want them, otherwise
|
||||||
|
the defaults for the peripheral class are used.
|
||||||
|
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# This defines an output pin which will default to output LOW. It is a
|
||||||
|
# pin that enables peripheral power on this board.
|
||||||
|
|
||||||
|
PA8 VDD_5V_PERIPH_EN OUTPUT LOW
|
||||||
|
|
||||||
|
# This is the pin that senses USB being connected. It is an input pin
|
||||||
|
# setup as OPENDRAIN.
|
||||||
|
PA9 VBUS INPUT OPENDRAIN
|
||||||
|
|
||||||
|
# This is a commented out pin for talking to the debug UART on the
|
||||||
|
# IOMCU, not used yet, but left as a comment (with a '#' in front) for
|
||||||
|
# future reference
|
||||||
|
# PA10 IO-debug-console
|
||||||
|
|
||||||
|
# Now we define the pins that USB is connected on.
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
# These are the pins for SWD debugging with a STlinkv2 or black-magic probe.
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
# This defines the PWM pin for the buzzer (if there is one). It is
|
||||||
|
# also mapped to a GPIO output so you can play with the buzzer via
|
||||||
|
# MAVLink relay commands if you want to.
|
||||||
|
|
||||||
|
# PWM output for buzzer
|
||||||
|
PA15 TIM2_CH1 TIM2 GPIO(77) ALARM
|
||||||
|
|
||||||
|
# This defines a couple of general purpose outputs, mapped to GPIO
|
||||||
|
# numbers 1 and 2 for users.
|
||||||
|
PB0 EXTERN_GPIO1 OUTPUT GPIO(1)
|
||||||
|
PB1 EXTERN_GPIO2 OUTPUT GPIO(2)
|
||||||
|
|
||||||
|
# This defines some input pins, currently unused.
|
||||||
|
PB2 BOOT1 INPUT
|
||||||
|
PB3 FMU_SW0 INPUT
|
||||||
|
|
||||||
|
# This defines the pins for the 2nd CAN interface, if available.
|
||||||
|
PB6 CAN2_TX CAN2
|
||||||
|
PB12 CAN2_RX CAN2
|
||||||
|
|
||||||
|
# Now the first I2C bus. The pin speeds are automatically setup
|
||||||
|
# correctly, but can be overridden here if needed.
|
||||||
|
PB8 I2C1_SCL I2C1
|
||||||
|
PB9 I2C1_SDA I2C1
|
||||||
|
|
||||||
|
# the 2nd I2C bus
|
||||||
|
PB10 I2C2_SCL I2C2
|
||||||
|
PB11 I2C2_SDA I2C2
|
||||||
|
|
||||||
|
# the 2nd SPI bus
|
||||||
|
PB13 SPI2_SCK SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
|
||||||
|
# This input pin is used to detect that power is valid on USB.
|
||||||
|
PC0 VBUS_nVALID INPUT PULLUP
|
||||||
|
|
||||||
|
# This defines the CS pin for the magnetometer and first IMU. Note
|
||||||
|
# that CS pins are software controlled, and are not tied to a particular
|
||||||
|
# SPI bus.
|
||||||
|
PC1 MAG_CS CS
|
||||||
|
PC2 MPU_CS CS
|
||||||
|
|
||||||
|
# This defines more ADC inputs.
|
||||||
|
PC3 AUX_POWER ADC1 SCALE(1)
|
||||||
|
PC4 AUX_ADC2 ADC1 SCALE(1)
|
||||||
|
|
||||||
|
# And the analog input for airspeed (rarely used these days).
|
||||||
|
PC5 PRESSURE_SENS ADC1 SCALE(2)
|
||||||
|
|
||||||
|
# This sets up the UART for talking to the IOMCU. Note that it is
|
||||||
|
# vital that this UART has DMA available. See the DMA settings below
|
||||||
|
# for more information.
|
||||||
|
|
||||||
|
# USART6 to IO
|
||||||
|
PC6 USART6_TX USART6
|
||||||
|
PC7 USART6_RX USART6
|
||||||
|
|
||||||
|
# Now setup the pins for the microSD card, if available.
|
||||||
|
PC8 SDIO_D0 SDIO
|
||||||
|
PC9 SDIO_D1 SDIO
|
||||||
|
PC10 SDIO_D2 SDIO
|
||||||
|
PC11 SDIO_D3 SDIO
|
||||||
|
PC12 SDIO_CK SDIO
|
||||||
|
PD2 SDIO_CMD SDIO
|
||||||
|
|
||||||
|
# More CS pins for more sensors. The labels for all CS pins need to
|
||||||
|
# match the SPI device table later in this file.
|
||||||
|
PC13 GYRO_EXT_CS CS
|
||||||
|
PC14 BARO_EXT_CS CS
|
||||||
|
PC15 ACCEL_EXT_CS CS
|
||||||
|
PD7 BARO_CS CS
|
||||||
|
PE4 MPU_EXT_CS CS
|
||||||
|
|
||||||
|
# the first CAN bus
|
||||||
|
PD0 CAN1_RX CAN1
|
||||||
|
PD1 CAN1_TX CAN1
|
||||||
|
|
||||||
|
# Another USART, this one for telem1. This one has RTS and CTS lines.
|
||||||
|
# USART2 serial2 telem1
|
||||||
|
PD3 USART2_CTS USART2
|
||||||
|
PD4 USART2_RTS USART2
|
||||||
|
PD5 USART2_TX USART2
|
||||||
|
PD6 USART2_RX USART2
|
||||||
|
|
||||||
|
# The telem2 USART, also with RTS/CTS available.
|
||||||
|
# USART3 serial3 telem2
|
||||||
|
PD8 USART3_TX USART3
|
||||||
|
PD9 USART3_RX USART3
|
||||||
|
PD11 USART3_CTS USART3
|
||||||
|
PD12 USART3_RTS USART3
|
||||||
|
|
||||||
|
# The CS pin for FRAM (ramtron). This one is marked as using
|
||||||
|
# SPEED_VERYLOW, which matches the HAL_PX4 setup.
|
||||||
|
PD10 FRAM_CS CS SPEED_VERYLOW
|
||||||
|
|
||||||
|
# Now we start defining some PWM pins. We also map these pins to GPIO
|
||||||
|
# values, so users can set BRD_PWM_COUNT to choose how many of the PWM
|
||||||
|
# outputs on the primary MCU are setup as PWM and how many as
|
||||||
|
# GPIOs. To match HAL_PX4 we number the GPIOs for the PWM outputs
|
||||||
|
# starting at 50.
|
||||||
|
PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) # one of these is require by SPI4
|
||||||
|
PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51) BIDIR
|
||||||
|
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR
|
||||||
|
PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53)
|
||||||
|
PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) NODMA
|
||||||
|
PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) NODMA
|
||||||
|
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 4
|
||||||
|
|
||||||
|
# Relays default to use GPIO pins 54 and 55.
|
||||||
|
define RELAY1_PIN_DEFAULT 54
|
||||||
|
define RELAY2_PIN_DEFAULT 55
|
||||||
|
|
||||||
|
# This is the invensense data-ready pin. We don't use it in the
|
||||||
|
# default driver.
|
||||||
|
PD15 MPU_DRDY INPUT
|
||||||
|
|
||||||
|
# the 2nd GPS UART
|
||||||
|
# UART8 serial4 GPS2
|
||||||
|
PE0 UART8_RX UART8
|
||||||
|
PE1 UART8_TX UART8
|
||||||
|
|
||||||
|
# Now setup SPI bus4.
|
||||||
|
#PE2 SPI4_SCK SPI4
|
||||||
|
#PE5 SPI4_MISO SPI4
|
||||||
|
#PE6 SPI4_MOSI SPI4
|
||||||
|
|
||||||
|
# This is the pin to enable the sensors rail. It can be used to power
|
||||||
|
# cycle sensors to recover them in case there are problems with power on
|
||||||
|
# timing affecting sensor stability. We pull it high by default.
|
||||||
|
PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters).
|
||||||
|
PE7 UART7_RX UART7
|
||||||
|
PE8 UART7_TX UART7
|
||||||
|
|
||||||
|
# Define a LED, mapping it to GPIO(0). LOW will illuminate the LED
|
||||||
|
PE12 FMU_LED_AMBER OUTPUT HIGH OPENDRAIN GPIO(0)
|
||||||
|
|
||||||
|
# Power flag pins: these tell the MCU the status of the various power
|
||||||
|
# supplies that are available. The pin names need to exactly match the
|
||||||
|
# names used in AnalogIn.cpp.
|
||||||
|
PB5 VDD_BRICK_nVALID INPUT PULLUP
|
||||||
|
PB7 VDD_BRICK2_nVALID INPUT PULLUP
|
||||||
|
PE10 VDD_5V_HIPOWER_nOC INPUT PULLUP
|
||||||
|
PE15 VDD_5V_PERIPH_nOC INPUT PULLUP
|
||||||
|
|
||||||
|
# Now the SPI device table. This table creates all accessible SPI
|
||||||
|
# devices, giving the name of the device (which is used by device
|
||||||
|
# drivers to open the device), plus which SPI bus it it on, what
|
||||||
|
# device ID will be used (which controls the IDs used in parameters
|
||||||
|
# such as COMPASS_DEV_ID, so we can detect when the list of devices
|
||||||
|
# changes between reboots for calibration purposes), the SPI mode to
|
||||||
|
# use, and the low and high speed settings for the device.
|
||||||
|
|
||||||
|
# You can define more SPI devices than you actually have, to allow for
|
||||||
|
# flexibility in board setup, and the driver code can probe to see
|
||||||
|
# which are responding.
|
||||||
|
|
||||||
|
# The DEVID values and device names are chosen to match the PX4 port
|
||||||
|
# of ArduPilot so users don't need to re-do their accel and compass
|
||||||
|
# calibrations when moving to ChibiOS.
|
||||||
|
|
||||||
|
SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ
|
||||||
|
#SPIDEV ms5611_ext SPI4 DEVID2 BARO_EXT_CS MODE3 20*MHZ 20*MHZ
|
||||||
|
SPIDEV mpu6000 SPI1 DEVID4 MPU_CS MODE3 2*MHZ 8*MHZ
|
||||||
|
SPIDEV icm20608-am SPI1 DEVID2 ACCEL_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV mpu9250 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
#SPIDEV mpu9250_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
#SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lsm9ds0_g SPI1 DEVID1 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
SPIDEV lsm9ds0_am SPI1 DEVID2 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
#SPIDEV lsm9ds0_ext_g SPI4 DEVID4 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
#SPIDEV lsm9ds0_ext_am SPI4 DEVID3 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ
|
||||||
|
#SPIDEV icm20602_ext SPI4 DEVID4 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ
|
||||||
|
SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ
|
||||||
|
#SPIDEV external0m0 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ
|
||||||
|
#SPIDEV external0m1 SPI4 DEVID5 MPU_EXT_CS MODE1 2*MHZ 2*MHZ
|
||||||
|
#SPIDEV external0m2 SPI4 DEVID5 MPU_EXT_CS MODE2 2*MHZ 2*MHZ
|
||||||
|
#SPIDEV external0m3 SPI4 DEVID5 MPU_EXT_CS MODE3 2*MHZ 2*MHZ
|
||||||
|
#SPIDEV pixartPC15 SPI4 DEVID13 ACCEL_EXT_CS MODE3 2*MHZ 2*MHZ
|
||||||
|
|
||||||
|
# Now some commented out SPI device names which can be used by
|
||||||
|
# developers to test that the clock calculations are right for a
|
||||||
|
# bus. This is used in conjunction with the mavproxy devop module.
|
||||||
|
|
||||||
|
# for SPI clock testing
|
||||||
|
#SPIDEV clock500 SPI4 DEVID5 MPU_EXT_CS MODE0 500*KHZ 500*KHZ # gives 329KHz
|
||||||
|
#SPIDEV clock1 SPI4 DEVID5 MPU_EXT_CS MODE0 1*MHZ 1*MHZ # gives 657kHz
|
||||||
|
#SPIDEV clock2 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ # gives 1.3MHz
|
||||||
|
#SPIDEV clock4 SPI4 DEVID5 MPU_EXT_CS MODE0 4*MHZ 4*MHZ # gives 2.6MHz
|
||||||
|
#SPIDEV clock8 SPI4 DEVID5 MPU_EXT_CS MODE0 8*MHZ 8*MHZ # gives 5.5MHz
|
||||||
|
#SPIDEV clock16 SPI4 DEVID5 MPU_EXT_CS MODE0 16*MHZ 16*MHZ # gives 10.6MHz
|
||||||
|
|
||||||
|
# This adds a C define which sets up the ArduPilot architecture
|
||||||
|
# define. Any line starting with 'define' is copied literally as
|
||||||
|
# a #define in the hwdef.h header.
|
||||||
|
define HAL_CHIBIOS_ARCH_FMUV3 1
|
||||||
|
|
||||||
|
# Nnow some defines for logging and terrain data files.
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
# We need to tell HAL_ChibiOS/Storage.cpp how much storage is
|
||||||
|
# available (in bytes).
|
||||||
|
define HAL_STORAGE_SIZE 16384
|
||||||
|
|
||||||
|
# allow to have have a dedicated safety switch pin
|
||||||
|
define HAL_HAVE_SAFETY_SWITCH 1
|
||||||
|
|
||||||
|
# This enables the use of a ramtron device for storage, if one is
|
||||||
|
# found on SPI. You must have a ramtron entry in the SPI device table.
|
||||||
|
|
||||||
|
# Enable RAMTROM parameter storage.
|
||||||
|
define HAL_WITH_RAMTRON 1
|
||||||
|
|
||||||
|
# Setup for the possibility of an IMU heater since the pixhawk2 cube has
|
||||||
|
# an IMU heater.
|
||||||
|
define HAL_HAVE_IMU_HEATER 1
|
||||||
|
|
||||||
|
# Enable FAT filesystem support (needs a microSD defined via SDIO).
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
|
||||||
|
# Now setup the default battery pins driver analog pins and default
|
||||||
|
# scaling for the power brick.
|
||||||
|
define HAL_BATT_VOLT_PIN 2
|
||||||
|
define HAL_BATT_CURR_PIN 3
|
||||||
|
define HAL_BATT_VOLT_SCALE 10.1
|
||||||
|
define HAL_BATT_CURR_SCALE 17.0
|
||||||
|
|
||||||
|
# This defines the default maximum clock on I2C devices.
|
||||||
|
define HAL_I2C_MAX_CLOCK 100000
|
||||||
|
|
||||||
|
# We can't share the IO UART (USART6).
|
||||||
|
DMA_NOSHARE USART6_TX ADC1
|
||||||
|
DMA_PRIORITY TIM1_CH3 TIM1_CH2 TIM1_UP USART6* ADC1* SDIO* SPI*
|
||||||
|
|
||||||
|
# List of files to put in ROMFS. For fmuv3 we need an IO firmware so
|
||||||
|
# we can automatically update the IOMCU firmware on boot. The format
|
||||||
|
# is "ROMFS ROMFS-filename source-filename". Paths are relative to the
|
||||||
|
# ardupilot root.
|
||||||
|
ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_lowpolh.bin
|
321
libraries/AP_HAL_ChibiOS/hwdef/fmuv5-bdshot/hwdef.dat
Normal file
321
libraries/AP_HAL_ChibiOS/hwdef/fmuv5-bdshot/hwdef.dat
Normal file
@ -0,0 +1,321 @@
|
|||||||
|
# hw definition file for processing by chibios_hwdef.py
|
||||||
|
# for FMUv5 hardware (ie. for CUAV-PixHack-v5 and Pixhawk4)
|
||||||
|
|
||||||
|
# MCU class and specific type. It is a F765, which is the same as F767
|
||||||
|
# but without the TFT interface
|
||||||
|
MCU STM32F7xx STM32F767xx
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 16000000
|
||||||
|
|
||||||
|
define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_CHIBIOS_FMUV5
|
||||||
|
define HAL_CHIBIOS_ARCH_FMUV5 1
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 50
|
||||||
|
|
||||||
|
FLASH_RESERVE_START_KB 32
|
||||||
|
|
||||||
|
|
||||||
|
# flash size
|
||||||
|
FLASH_SIZE_KB 2048
|
||||||
|
|
||||||
|
env OPTIMIZE -O2
|
||||||
|
|
||||||
|
# order of UARTs (and USB)
|
||||||
|
SERIAL_ORDER OTG1 USART2 USART3 USART1 UART4 USART6 UART7 OTG2
|
||||||
|
|
||||||
|
# default the 2nd interface to MAVLink2 until MissionPlanner updates drivers
|
||||||
|
define HAL_OTG2_PROTOCOL SerialProtocol_MAVLink2
|
||||||
|
|
||||||
|
# now we define the pins that USB is connected on
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
# these are the pins for SWD debugging with a STlinkv2 or black-magic probe
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
# SPI1 - internal sensors
|
||||||
|
PG11 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PD7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# SPI2 - FRAM
|
||||||
|
PI1 SPI2_SCK SPI2
|
||||||
|
PI2 SPI2_MISO SPI2
|
||||||
|
PI3 SPI2_MOSI SPI2
|
||||||
|
|
||||||
|
# SPI4 - sensors2
|
||||||
|
PE2 SPI4_SCK SPI4
|
||||||
|
PE13 SPI4_MISO SPI4
|
||||||
|
PE6 SPI4_MOSI SPI4
|
||||||
|
|
||||||
|
# SPI5 - external1 (disabled to save DMA channels)
|
||||||
|
# PF7 SPI5_SCK SPI5
|
||||||
|
# PF8 SPI5_MISO SPI5
|
||||||
|
# PF9 SPI5_MOSI SPI5
|
||||||
|
|
||||||
|
# SPI6 - external2 (disabled to save DMA channels)
|
||||||
|
# PG13 SPI6_SCK SPI6
|
||||||
|
# PG12 SPI6_MISO SPI6
|
||||||
|
# PB5 SPI6_MOSI SPI6
|
||||||
|
|
||||||
|
# sensor CS
|
||||||
|
PF10 MS5611_CS CS
|
||||||
|
PF2 ICM20689_CS CS SPEED_VERYLOW
|
||||||
|
PF3 ICM20602_CS CS SPEED_VERYLOW
|
||||||
|
PF4 BMI055_G_CS CS
|
||||||
|
PG10 BMI055_A_CS CS
|
||||||
|
PF5 FRAM_CS CS SPEED_VERYLOW
|
||||||
|
|
||||||
|
# unusued CS pins
|
||||||
|
PF11 SPARE_CS CS
|
||||||
|
PH5 AUXMEM_CS CS
|
||||||
|
PI4 EXTERNAL1_CS1 CS
|
||||||
|
PI10 EXTERNAL1_CS2 CS
|
||||||
|
PI11 EXTERNAL1_CS3 CS
|
||||||
|
PI6 EXTERNAL2_CS1 CS
|
||||||
|
PI7 EXTERNAL2_CS2 CS
|
||||||
|
PI8 EXTERNAL2_CS3 CS
|
||||||
|
|
||||||
|
# I2C buses
|
||||||
|
|
||||||
|
PB8 I2C1_SCL I2C1
|
||||||
|
PB9 I2C1_SDA I2C1
|
||||||
|
|
||||||
|
PF1 I2C2_SCL I2C2
|
||||||
|
PF0 I2C2_SDA I2C2
|
||||||
|
|
||||||
|
PH7 I2C3_SCL I2C3
|
||||||
|
PH8 I2C3_SDA I2C3
|
||||||
|
|
||||||
|
PF14 I2C4_SCL I2C4
|
||||||
|
PF15 I2C4_SDA I2C4
|
||||||
|
|
||||||
|
# order of I2C buses
|
||||||
|
I2C_ORDER I2C3 I2C1 I2C2 I2C4
|
||||||
|
|
||||||
|
|
||||||
|
# enable pins
|
||||||
|
PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# start peripheral power off, then enable after init
|
||||||
|
# this prevents a problem with radios that use RTS for
|
||||||
|
# bootloader hold
|
||||||
|
PF12 nVDD_5V_HIPOWER_EN OUTPUT HIGH
|
||||||
|
PG4 nVDD_5V_PERIPH_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
PG5 VDD_5V_RC_EN OUTPUT HIGH
|
||||||
|
PG6 VDD_5V_WIFI_EN OUTPUT HIGH
|
||||||
|
PG7 VDD_3V3_SD_CARD_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# drdy pins
|
||||||
|
PE7 DRDY8_NC INPUT
|
||||||
|
PB4 DRDY1_ICM20689 INPUT
|
||||||
|
PB14 DRDY2_BMI055_GYRO INPUT
|
||||||
|
PB15 DRDY3_BMI055_ACC INPUT
|
||||||
|
PC5 DRDY4_ICM20602 INPUT
|
||||||
|
PC13 DRDY5_BMI055_GYRO INPUT
|
||||||
|
PD10 DRDY6_BMI055_ACC INPUT
|
||||||
|
PD15 DRDY7_EXTERNAL1 INPUT
|
||||||
|
|
||||||
|
# Control of Spektrum power pin
|
||||||
|
PE4 SPEKTRUM_PWR OUTPUT HIGH GPIO(73)
|
||||||
|
define HAL_GPIO_SPEKTRUM_PWR 73
|
||||||
|
|
||||||
|
# Spektrum Power is Active High
|
||||||
|
define HAL_SPEKTRUM_PWR_ENABLED 1
|
||||||
|
|
||||||
|
# UARTs
|
||||||
|
|
||||||
|
# USART2 is telem1
|
||||||
|
PD6 USART2_RX USART2
|
||||||
|
PD5 USART2_TX USART2
|
||||||
|
PD3 USART2_CTS USART2
|
||||||
|
PD4 USART2_RTS USART2
|
||||||
|
|
||||||
|
# USART1 is GPS1
|
||||||
|
PB7 USART1_RX USART1 NODMA
|
||||||
|
PB6 USART1_TX USART1 NODMA
|
||||||
|
|
||||||
|
# USART3 is telem2
|
||||||
|
PD9 USART3_RX USART3
|
||||||
|
PD8 USART3_TX USART3
|
||||||
|
PD11 USART3_CTS USART3
|
||||||
|
PD12 USART3_RTS USART3
|
||||||
|
|
||||||
|
# UART4 GPS2
|
||||||
|
PD0 UART4_RX UART4 NODMA
|
||||||
|
PD1 UART4_TX UART4 NODMA
|
||||||
|
|
||||||
|
# USART6 is telem3
|
||||||
|
PG9 USART6_RX USART6 NODMA
|
||||||
|
# we leave PG14 as an input to prevent it acting as a pullup
|
||||||
|
# on the IOMCU SBUS input
|
||||||
|
# PG14 USART6_TX USART6 NODMA
|
||||||
|
PG15 USART6_CTS USART6
|
||||||
|
PG8 USART6_RTS USART6
|
||||||
|
|
||||||
|
# UART7 is debug
|
||||||
|
PF6 UART7_RX UART7 NODMA
|
||||||
|
PE8 UART7_TX UART7 NODMA
|
||||||
|
|
||||||
|
# UART8 is for IOMCU
|
||||||
|
PE0 UART8_RX UART8
|
||||||
|
PE1 UART8_TX UART8
|
||||||
|
|
||||||
|
# UART for IOMCU
|
||||||
|
IOMCU_UART UART8
|
||||||
|
|
||||||
|
# PWM AUX channels
|
||||||
|
PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50)
|
||||||
|
PA10 TIM1_CH3 TIM1 PWM(2) GPIO(51) BIDIR
|
||||||
|
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR
|
||||||
|
PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53)
|
||||||
|
PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) NODMA
|
||||||
|
PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) NODMA
|
||||||
|
# we need to disable DMA on the last 2 FMU channels
|
||||||
|
# as timer 12 doesn't have a TIMn_UP DMA option
|
||||||
|
PH6 TIM12_CH1 TIM12 PWM(7) GPIO(56) NODMA
|
||||||
|
PH9 TIM12_CH2 TIM12 PWM(8) GPIO(57) NODMA
|
||||||
|
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 8
|
||||||
|
|
||||||
|
# PWM output for buzzer
|
||||||
|
PE5 TIM9_CH1 TIM9 GPIO(77) ALARM
|
||||||
|
|
||||||
|
# analog in
|
||||||
|
PA0 BATT_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PA1 BATT_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
PA2 BATT2_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PA3 BATT2_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
PC4 SPARE1_ADC1 ADC1 SCALE(1)
|
||||||
|
PA4 SPARE2_ADC1 ADC1 SCALE(1)
|
||||||
|
|
||||||
|
PB0 RSSI_IN ADC1 SCALE(1)
|
||||||
|
|
||||||
|
#PC3 HW_REV_SENS ADC1 SCALE(1)
|
||||||
|
#PC2 HW_VER_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
PC0 VDD_5V_SENS ADC1 SCALE(2)
|
||||||
|
PC1 SCALED_V3V3 ADC1 SCALE(2)
|
||||||
|
|
||||||
|
# setup scaling defaults for PixHackV5 power brick
|
||||||
|
define HAL_BATT_VOLT_SCALE 18.0
|
||||||
|
define HAL_BATT_CURR_SCALE 24.0
|
||||||
|
define HAL_BATT_VOLT_PIN 0
|
||||||
|
define HAL_BATT_CURR_PIN 1
|
||||||
|
define HAL_BATT2_VOLT_PIN 2
|
||||||
|
define HAL_BATT2_CURR_PIN 3
|
||||||
|
|
||||||
|
# CAN bus
|
||||||
|
PI9 CAN1_RX CAN1
|
||||||
|
PH13 CAN1_TX CAN1
|
||||||
|
|
||||||
|
PB12 CAN2_RX CAN2
|
||||||
|
PB13 CAN2_TX CAN2
|
||||||
|
|
||||||
|
PH2 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70)
|
||||||
|
PH3 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(71)
|
||||||
|
PH4 GPIO_CAN3_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(72)
|
||||||
|
|
||||||
|
# GPIOs
|
||||||
|
PA7 HEATER_EN OUTPUT LOW GPIO(80)
|
||||||
|
define HAL_HEATER_GPIO_PIN 80
|
||||||
|
|
||||||
|
PG1 VDD_BRICK_nVALID INPUT PULLUP
|
||||||
|
PG2 VDD_BRICK2_nVALID INPUT PULLUP
|
||||||
|
PG3 VBUS_nVALID INPUT PULLUP
|
||||||
|
PF13 VDD_5V_HIPOWER_nOC INPUT PULLUP
|
||||||
|
PE15 VDD_5V_PERIPH_nOC INPUT PULLUP
|
||||||
|
PB10 nSPI5_RESET_EXTERNAL1 OUTPUT HIGH
|
||||||
|
|
||||||
|
# capture pins
|
||||||
|
PA5 FMU_CAP1 INPUT GPIO(58)
|
||||||
|
PB3 FMU_CAP2 INPUT GPIO(59)
|
||||||
|
PB11 FMU_CAP3 INPUT GPIO(60)
|
||||||
|
PI0 FMU_SPARE_4 INPUT GPIO(61)
|
||||||
|
|
||||||
|
# SPI devices
|
||||||
|
SPIDEV ms5611 SPI4 DEVID1 MS5611_CS MODE3 20*MHZ 20*MHZ
|
||||||
|
SPIDEV icm20689 SPI1 DEVID1 ICM20689_CS MODE3 2*MHZ 8*MHZ
|
||||||
|
SPIDEV icm20602 SPI1 DEVID2 ICM20602_CS MODE3 2*MHZ 8*MHZ
|
||||||
|
SPIDEV bmi055_g SPI1 DEVID3 BMI055_G_CS MODE3 10*MHZ 10*MHZ
|
||||||
|
SPIDEV bmi055_a SPI1 DEVID4 BMI055_A_CS MODE3 10*MHZ 10*MHZ
|
||||||
|
SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ
|
||||||
|
|
||||||
|
# up to 3 IMUs
|
||||||
|
IMU Invensense SPI:icm20689 ROTATION_NONE
|
||||||
|
IMU Invensense SPI:icm20602 ROTATION_NONE
|
||||||
|
# 3rd could be BMMI055 or BMI088
|
||||||
|
IMU BMI055 SPI:bmi055_a SPI:bmi055_g ROTATION_ROLL_180_YAW_90
|
||||||
|
IMU BMI088 SPI:bmi055_a SPI:bmi055_g ROTATION_ROLL_180_YAW_90
|
||||||
|
|
||||||
|
define HAL_DEFAULT_INS_FAST_SAMPLE 1
|
||||||
|
|
||||||
|
# probe external I2C compasses plus some internal IST8310
|
||||||
|
# we also probe some external IST8310 with a non-standard orientation
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90
|
||||||
|
COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_ROLL_180_YAW_90
|
||||||
|
|
||||||
|
# one baro
|
||||||
|
BARO MS56XX SPI:ms5611
|
||||||
|
|
||||||
|
# microSD support
|
||||||
|
PC8 SDMMC_D0 SDMMC1
|
||||||
|
PC9 SDMMC_D1 SDMMC1
|
||||||
|
PC10 SDMMC_D2 SDMMC1
|
||||||
|
PC11 SDMMC_D3 SDMMC1
|
||||||
|
PC12 SDMMC_CK SDMMC1
|
||||||
|
PD2 SDMMC_CMD SDMMC1
|
||||||
|
|
||||||
|
# red LED marked as B/E
|
||||||
|
PB1 LED_RED OUTPUT OPENDRAIN GPIO(90)
|
||||||
|
|
||||||
|
# green LED marked as PWR. We leave this solid on, but allow
|
||||||
|
# for it to be controlled as a relay if needed
|
||||||
|
PC6 LED_GREEN OUTPUT GPIO(91) LOW
|
||||||
|
|
||||||
|
# blue LED marked as ACT
|
||||||
|
PC7 LED_BLUE OUTPUT GPIO(92) HIGH
|
||||||
|
|
||||||
|
# setup for BoardLED2
|
||||||
|
define HAL_GPIO_A_LED_PIN 90
|
||||||
|
define HAL_GPIO_B_LED_PIN 92
|
||||||
|
define HAL_GPIO_LED_ON 0
|
||||||
|
|
||||||
|
|
||||||
|
# enable RAMTROM parameter storage
|
||||||
|
define HAL_STORAGE_SIZE 16384
|
||||||
|
define HAL_WITH_RAMTRON 1
|
||||||
|
|
||||||
|
# allow to have have a dedicated safety switch pin
|
||||||
|
define HAL_HAVE_SAFETY_SWITCH 1
|
||||||
|
|
||||||
|
define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
|
||||||
|
define HAL_COMPASS_AUTO_ROT_DEFAULT 2
|
||||||
|
|
||||||
|
DMA_PRIORITY TIM1_CH3 TIM1_CH2 TIM1_UP SDMMC* UART8* ADC* SPI* TIM*
|
||||||
|
|
||||||
|
# define HAL_SPI_CHECK_CLOCK_FREQ 1
|
||||||
|
|
||||||
|
# enable FAT filesystem support (needs a microSD defined via SDMMC)
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_lowpolh.bin
|
||||||
|
|
||||||
|
# uncomment block below to enable a 2nd MS5611 baro on SPI5
|
||||||
|
#PF7 SPI5_SCK SPI5
|
||||||
|
#PF8 SPI5_MISO SPI5
|
||||||
|
#PF9 SPI5_MOSI SPI5
|
||||||
|
#SPIDEV ms5611_spi5 SPI5 DEVID1 EXTERNAL1_CS1 MODE3 20*MHZ 20*MHZ
|
||||||
|
#undef PI10
|
||||||
|
#PI10 EXTERNAL1_CS2 OUTPUT LOW
|
||||||
|
#BARO MS56XX SPI:ms5611_spi5
|
@ -0,0 +1,7 @@
|
|||||||
|
#Default Parameters for the mRo Pixracer Pro
|
||||||
|
|
||||||
|
CAN_P1_DRIVER 1
|
||||||
|
CAN_P2_DRIVER 2
|
||||||
|
CAN_SLCAN_CPORT 1
|
||||||
|
SERIAL6_PROTOCOL 22
|
||||||
|
|
285
libraries/AP_HAL_ChibiOS/hwdef/mRoPixracerPro-bdshot/hwdef.dat
Normal file
285
libraries/AP_HAL_ChibiOS/hwdef/mRoPixracerPro-bdshot/hwdef.dat
Normal file
@ -0,0 +1,285 @@
|
|||||||
|
###########################################################################################################################################################
|
||||||
|
# mRo Pixracer Pro Flight Controller
|
||||||
|
# STM32H743IIK6
|
||||||
|
# 8 PWM / IO - DMA capable and hardware and/or software switchable 3.3v / 5v Logic (hardware default)
|
||||||
|
# Logic level set in hardware: No Solder for 5v, Solder 1-2 for 3.3v, Solder 2-3 for 3.3v / 5v software switchable
|
||||||
|
# Exposed JST GH Connectors: GPS UART+I2C connector, 6 Available UARTS (2 with Flow Control), 2 FDCAN, SPI, I2C, Power with analog current and voltage sense
|
||||||
|
# 3 IMUs (ICM20602 6DOF, ICM20948 9DOF, BMI088 6DOF)
|
||||||
|
# Baro, FRAM (256kb), SDCARD Socket, TC2030 JTAG, USB-C
|
||||||
|
# Onboard 3 color LED and buzzer
|
||||||
|
# Uncased weight and dimensions:
|
||||||
|
# Weight (with headers): 11.3g (.40oz)
|
||||||
|
# Length: 36mm (1.42in)
|
||||||
|
# Width: 36mm (1.42in)
|
||||||
|
# Mounting holes: 31.5mm x 31.5mm 4mm grommeted mounting holes for 3mm fasteners
|
||||||
|
# M10064C - Initial Release
|
||||||
|
###########################################################################################################################################################
|
||||||
|
|
||||||
|
# default to all pins low to avoid ESD issues
|
||||||
|
DEFAULTGPIO OUTPUT LOW PULLDOWN
|
||||||
|
|
||||||
|
# MCU class and specific type
|
||||||
|
MCU STM32H7xx STM32H743xx
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 1017
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 24000000
|
||||||
|
|
||||||
|
FLASH_SIZE_KB 2048
|
||||||
|
|
||||||
|
# with 2M flash we can afford to optimize for speed
|
||||||
|
env OPTIMIZE -O2
|
||||||
|
|
||||||
|
# start on 2th sector (1st sector for bootloader)
|
||||||
|
FLASH_RESERVE_START_KB 128
|
||||||
|
|
||||||
|
# use FRAM for storage
|
||||||
|
define HAL_STORAGE_SIZE 16384
|
||||||
|
define HAL_WITH_RAMTRON 1
|
||||||
|
|
||||||
|
# USB setup
|
||||||
|
USB_STRING_MANUFACTURER "mRo"
|
||||||
|
|
||||||
|
# RC Input set for Interrupt also USART6_RX for serial RC
|
||||||
|
PC7 TIM3_CH2 TIM3 RCININT FLOAT LOW
|
||||||
|
|
||||||
|
# Control of Spektrum power pin
|
||||||
|
PE4 SPEKTRUM_PWR OUTPUT LOW GPIO(70)
|
||||||
|
define HAL_GPIO_SPEKTRUM_PWR 70
|
||||||
|
|
||||||
|
# Spektrum Power is Active Low
|
||||||
|
define HAL_SPEKTRUM_PWR_ENABLED 0
|
||||||
|
|
||||||
|
# Spektrum RC Input pin, used as GPIO for bind for Satellite Receivers
|
||||||
|
PB0 SPEKTRUM_RC INPUT PULLUP GPIO(71)
|
||||||
|
define HAL_GPIO_SPEKTRUM_RC 71
|
||||||
|
|
||||||
|
# Order of I2C buses
|
||||||
|
I2C_ORDER I2C1
|
||||||
|
|
||||||
|
# this board only has a single I2C bus so make it external
|
||||||
|
define HAL_I2C_INTERNAL_MASK 0
|
||||||
|
|
||||||
|
# order of UARTs (and USB) and suggested usage
|
||||||
|
# USART2 Telem 1 (Flow Control)
|
||||||
|
# USART3 Telem 2 (Flow Control)
|
||||||
|
# UART4 GPS
|
||||||
|
# UART8 FRSKY Telem
|
||||||
|
# USART1 Additional UART
|
||||||
|
# UART7 Additional UART
|
||||||
|
|
||||||
|
# USART6 RC input (Only RX pin is connected)
|
||||||
|
|
||||||
|
SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 USART1 UART7 OTG2
|
||||||
|
|
||||||
|
# default the 2nd interface to MAVLink2
|
||||||
|
define HAL_OTG2_PROTOCOL SerialProtocol_MAVLink2
|
||||||
|
|
||||||
|
# Another USART, this one for telem1. This one has RTS and CTS lines.
|
||||||
|
# USART2 telem1
|
||||||
|
PD3 USART2_CTS USART2
|
||||||
|
PD4 USART2_RTS USART2
|
||||||
|
PD5 USART2_TX USART2
|
||||||
|
PD6 USART2_RX USART2
|
||||||
|
|
||||||
|
# The telem2 USART, this one for telem2. This one has RTS and CTS lines.
|
||||||
|
# USART3 telem2
|
||||||
|
PD8 USART3_TX USART3
|
||||||
|
PD9 USART3_RX USART3
|
||||||
|
PD11 USART3_CTS USART3
|
||||||
|
PD12 USART3_RTS USART3
|
||||||
|
|
||||||
|
# UART4 GPS
|
||||||
|
PA0 UART4_TX UART4
|
||||||
|
PA1 UART4_RX UART4
|
||||||
|
|
||||||
|
# USART1 Spare
|
||||||
|
PB6 USART1_TX USART1
|
||||||
|
PB7 USART1_RX USART1
|
||||||
|
|
||||||
|
# UART7 Spare or Debug Console
|
||||||
|
PE7 UART7_RX UART7
|
||||||
|
PE8 UART7_TX UART7
|
||||||
|
|
||||||
|
# UART8 FrSky Telemetry
|
||||||
|
PE0 UART8_RX UART8
|
||||||
|
PE1 UART8_TX UART8
|
||||||
|
|
||||||
|
# RSSI Analog Input
|
||||||
|
PC1 RSSI_IN ADC1
|
||||||
|
|
||||||
|
# Analog current and voltage sense pins.
|
||||||
|
PA2 BATT_VOLTAGE_SENS ADC1 SCALE(1)
|
||||||
|
PA3 BATT_CURRENT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
# Now the VDD sense pin. This is used to sense primary board voltage.
|
||||||
|
PA4 VDD_5V_SENS ADC1 SCALE(2)
|
||||||
|
|
||||||
|
#SPI1 ICM_20602 / ICM_20948
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
#SPI2 FRAM / DPS310
|
||||||
|
PB10 SPI2_SCK SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
|
||||||
|
#SPI5 BMI088
|
||||||
|
PF7 SPI5_SCK SPI5
|
||||||
|
PF8 SPI5_MISO SPI5
|
||||||
|
PF9 SPI5_MOSI SPI5
|
||||||
|
|
||||||
|
#SPI6 EXTERNAL
|
||||||
|
PG13 SPI6_SCK SPI6
|
||||||
|
PG12 SPI6_MISO SPI6
|
||||||
|
PG14 SPI6_MOSI SPI6
|
||||||
|
|
||||||
|
# This is the pin that senses USB being connected. It is an input pin
|
||||||
|
# setup as OPENDRAIN.
|
||||||
|
PA9 VBUS INPUT OPENDRAIN
|
||||||
|
|
||||||
|
# This input pin is used to detect that power is valid on USB.
|
||||||
|
PC0 VBUS_VALID INPUT
|
||||||
|
|
||||||
|
# Now we define the pins that USB is connected on.
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
|
||||||
|
# These are the pins for SWD debugging with a STlinkv2 or black-magic probe.
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
# PWM output for buzzer
|
||||||
|
PA15 TIM2_CH1 TIM2 GPIO(77) ALARM
|
||||||
|
|
||||||
|
# Now the first I2C bus. The pin speeds are automatically setup
|
||||||
|
# correctly, but can be overridden here if needed.
|
||||||
|
PB8 I2C1_SCL I2C1
|
||||||
|
PB9 I2C1_SDA I2C1
|
||||||
|
|
||||||
|
# Now setup the pins for the microSD card, if available.
|
||||||
|
PC8 SDMMC1_D0 SDMMC1
|
||||||
|
PC9 SDMMC1_D1 SDMMC1
|
||||||
|
PC10 SDMMC1_D2 SDMMC1
|
||||||
|
PC11 SDMMC1_D3 SDMMC1
|
||||||
|
PC12 SDMMC1_CK SDMMC1
|
||||||
|
PD2 SDMMC1_CMD SDMMC1
|
||||||
|
|
||||||
|
# More CS pins for more sensors. The labels for all CS pins need to
|
||||||
|
# match the SPI device table later in this file.
|
||||||
|
PC2 ICM_20602_CS CS
|
||||||
|
PD7 BARO_CS CS
|
||||||
|
PD10 FRAM_CS CS SPEED_VERYLOW
|
||||||
|
PE15 ICM_20948_CS CS
|
||||||
|
PF10 BMI088_GYRO_CS CS
|
||||||
|
PF6 BMI088_ACCEL_CS CS
|
||||||
|
PG9 EXT_SPI_CS CS
|
||||||
|
|
||||||
|
# the first CAN bus
|
||||||
|
PD0 CAN1_RX CAN1
|
||||||
|
PD1 CAN1_TX CAN1
|
||||||
|
|
||||||
|
PF11 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(72)
|
||||||
|
|
||||||
|
# This defines the pins for the 2nd CAN interface, OEM Only.
|
||||||
|
PB13 CAN2_TX CAN2
|
||||||
|
PB12 CAN2_RX CAN2
|
||||||
|
|
||||||
|
PF14 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(73)
|
||||||
|
|
||||||
|
# Now we start defining some PWM pins. We also map these pins to GPIO
|
||||||
|
# values, so users can set BRD_PWM_COUNT to choose how many of the PWM
|
||||||
|
# outputs on the primary MCU are setup as PWM and how many as
|
||||||
|
# GPIOs. To match HAL_PX4 we number the GPIOs for the PWM outputs
|
||||||
|
# starting at 50.
|
||||||
|
PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) BIDIR
|
||||||
|
PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51)
|
||||||
|
PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52)
|
||||||
|
PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) BIDIR
|
||||||
|
PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54)
|
||||||
|
PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55)
|
||||||
|
PI5 TIM8_CH1 TIM8 PWM(7) GPIO(56)
|
||||||
|
PI6 TIM8_CH2 TIM8 PWM(8) GPIO(57)
|
||||||
|
|
||||||
|
DMA_PRIORITY TIM1_UP TIM1_CH4 TIM1_CH1
|
||||||
|
DMA_NOSHARE SPI1* SPI5* TIM1_UP
|
||||||
|
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 8
|
||||||
|
|
||||||
|
# This is the invensense data-ready pin. We don't use it in the
|
||||||
|
# default driver.
|
||||||
|
PD15 MPU_DRDY INPUT
|
||||||
|
|
||||||
|
# This is the pin to enable the sensors rail. It can be used to power
|
||||||
|
# cycle sensors to recover them in case there are problems with power on
|
||||||
|
# timing affecting sensor stability. We pull it high by default.
|
||||||
|
PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# 1.8V Sensor Level Shifter Output Enable. We pull it high by default.
|
||||||
|
PI3 VDD_1V8_SENSORS_EN OUTPUT HIGH
|
||||||
|
|
||||||
|
# Pin for PWM Voltage Selection, 0 means 3.3v, 1 means 5v
|
||||||
|
PG6 PWM_VOLT_SEL OUTPUT LOW GPIO(74)
|
||||||
|
define HAL_GPIO_PWM_VOLT_PIN 74
|
||||||
|
define HAL_GPIO_PWM_VOLT_3v3 0
|
||||||
|
|
||||||
|
# Power flag pins: these tell the MCU the status of the various power
|
||||||
|
# supplies that are available. The pin names need to exactly match the
|
||||||
|
# names used in AnalogIn.cpp.
|
||||||
|
PB5 VDD_BRICK_VALID INPUT PULLUP
|
||||||
|
|
||||||
|
SPIDEV icm20948 SPI1 DEVID1 ICM_20948_CS MODE3 2*MHZ 4*MHZ
|
||||||
|
SPIDEV icm20608 SPI1 DEVID2 ICM_20602_CS MODE3 2*MHZ 8*MHZ
|
||||||
|
SPIDEV dps310 SPI2 DEVID3 BARO_CS MODE3 5*MHZ 5*MHZ
|
||||||
|
SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ
|
||||||
|
SPIDEV bmi088_g SPI5 DEVID1 BMI088_GYRO_CS MODE3 10*MHZ 10*MHZ
|
||||||
|
SPIDEV bmi088_a SPI5 DEVID2 BMI088_ACCEL_CS MODE3 10*MHZ 10*MHZ
|
||||||
|
|
||||||
|
# Now some defines for logging and terrain data files.
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
# Enable FAT filesystem support (needs a microSD defined via SDMMC).
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
|
||||||
|
# Now setup the default battery pins driver analog pins and default
|
||||||
|
# scaling for the power brick.
|
||||||
|
define HAL_BATT_VOLT_PIN 14
|
||||||
|
define HAL_BATT_CURR_PIN 15
|
||||||
|
define HAL_BATT_VOLT_SCALE 10.1
|
||||||
|
define HAL_BATT_CURR_SCALE 17.0
|
||||||
|
|
||||||
|
# No hardware safety pin, CAN safety switch only
|
||||||
|
# define HAL_HAVE_SAFETY_SWITCH 0
|
||||||
|
|
||||||
|
# Pixracer Pro has a TriColor LED, Red, Green, Blue
|
||||||
|
define HAL_HAVE_PIXRACER_LED
|
||||||
|
|
||||||
|
define HAL_GPIO_LED_ON 0
|
||||||
|
define HAL_GPIO_LED_OFF 1
|
||||||
|
|
||||||
|
# LED setup for PixracerLED driver
|
||||||
|
PB11 LED_R OUTPUT HIGH GPIO(0)
|
||||||
|
PB1 LED_G OUTPUT HIGH GPIO(1)
|
||||||
|
PB3 LED_B OUTPUT HIGH GPIO(2)
|
||||||
|
|
||||||
|
define HAL_GPIO_A_LED_PIN 0
|
||||||
|
define HAL_GPIO_B_LED_PIN 1
|
||||||
|
define HAL_GPIO_C_LED_PIN 2
|
||||||
|
|
||||||
|
# 3 IMUs
|
||||||
|
IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_NONE
|
||||||
|
IMU Invensense SPI:icm20608 ROTATION_ROLL_180_YAW_90
|
||||||
|
IMU Invensensev2 SPI:icm20948 ROTATION_ROLL_180_YAW_90
|
||||||
|
|
||||||
|
define HAL_DEFAULT_INS_FAST_SAMPLE 7
|
||||||
|
|
||||||
|
# 1 baro
|
||||||
|
BARO DPS280 SPI:dps310
|
||||||
|
|
||||||
|
# 1 compass
|
||||||
|
COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
163
libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro-bdshot/hwdef.dat
Normal file
163
libraries/AP_HAL_ChibiOS/hwdef/omnibusf4pro-bdshot/hwdef.dat
Normal file
@ -0,0 +1,163 @@
|
|||||||
|
# hw definition file for processing by chibios_pins.py
|
||||||
|
# Omnibus F4 PRO with on-board current sensor
|
||||||
|
# with F405 mcu, mpu6000 imu, bmp280 barometer, 7456 series osd and sdcard
|
||||||
|
|
||||||
|
MCU STM32F4xx STM32F405xx
|
||||||
|
|
||||||
|
HAL_CHIBIOS_ARCH_F405 1
|
||||||
|
|
||||||
|
# board ID for firmware load
|
||||||
|
APJ_BOARD_ID 131
|
||||||
|
|
||||||
|
# crystal frequency
|
||||||
|
OSCILLATOR_HZ 8000000
|
||||||
|
|
||||||
|
STM32_ST_USE_TIMER 5
|
||||||
|
|
||||||
|
# flash size
|
||||||
|
FLASH_SIZE_KB 1024
|
||||||
|
FLASH_RESERVE_START_KB 64
|
||||||
|
|
||||||
|
# order of I2C buses
|
||||||
|
I2C_ORDER I2C2
|
||||||
|
|
||||||
|
# order of UARTs
|
||||||
|
SERIAL_ORDER OTG1 USART1 USART3 USART6 UART4
|
||||||
|
|
||||||
|
#adc
|
||||||
|
PC1 BAT_CURR_SENS ADC1 SCALE(1)
|
||||||
|
PC2 BAT_VOLT_SENS ADC1 SCALE(1)
|
||||||
|
|
||||||
|
#analog rssi pin (also could be used as analog airspeed input)
|
||||||
|
PA0 RSSI_IN ADC1
|
||||||
|
define BOARD_RSSI_ANA_PIN 0
|
||||||
|
|
||||||
|
#pwm output. 1 - 4 on main header, 5 & 6 on separated header w/o 5V supply, 7 & 8 on CH5 and CH6 pads
|
||||||
|
PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50)
|
||||||
|
PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) BIDIR
|
||||||
|
PA3 TIM2_CH4 TIM2 PWM(3) GPIO(52) BIDIR
|
||||||
|
PA2 TIM2_CH3 TIM2 PWM(4) GPIO(53)
|
||||||
|
PA1 TIM2_CH2 TIM2 PWM(5) GPIO(54)
|
||||||
|
PA8 TIM1_CH1 TIM1 PWM(6) GPIO(55)
|
||||||
|
PC8 TIM8_CH3 TIM8 PWM(7) GPIO(56)
|
||||||
|
PC9 TIM8_CH4 TIM8 PWM(8) GPIO(59)
|
||||||
|
|
||||||
|
PA4 MPU6000_CS CS
|
||||||
|
PA5 SPI1_SCK SPI1
|
||||||
|
PA6 SPI1_MISO SPI1
|
||||||
|
PA7 SPI1_MOSI SPI1
|
||||||
|
|
||||||
|
# note that this board needs PULLUP on I2C pins
|
||||||
|
PB10 I2C2_SCL I2C2 PULLUP
|
||||||
|
PB11 I2C2_SDA I2C2 PULLUP
|
||||||
|
|
||||||
|
# use I2C pins as USART3 (SERIAL2) in BRD_ALT_CONFIG=1 & 4
|
||||||
|
PB10 USART3_TX USART3 ALT(1)
|
||||||
|
PB11 USART3_RX USART3 ALT(1)
|
||||||
|
PB10 USART3_TX USART3 ALT(4)
|
||||||
|
PB11 USART3_RX USART3 ALT(4)
|
||||||
|
|
||||||
|
PB15 SPI2_MOSI SPI2
|
||||||
|
PB14 SPI2_MISO SPI2
|
||||||
|
PB13 SPI2_SCK SPI2
|
||||||
|
PB12 SDCARD_CS CS
|
||||||
|
|
||||||
|
PA10 USART1_RX USART1
|
||||||
|
PA9 USART1_TX USART1
|
||||||
|
|
||||||
|
PC6 USART6_TX USART6
|
||||||
|
PC7 USART6_RX USART6
|
||||||
|
|
||||||
|
# UART4 TX available as an alternative config on PA0 (RSSI pad) with BRD_ALT_CONFIG=2
|
||||||
|
PA0 UART4_TX UART4 ALT(2)
|
||||||
|
|
||||||
|
# full UART4 also available as alt config on PA0 (RSSI pad) and PA1 (PWM output chan 5) with BRD_ALT_CONFIG=3 & 4
|
||||||
|
PA0 UART4_TX UART4 ALT(3)
|
||||||
|
PA1 UART4_RX UART4 ALT(3)
|
||||||
|
PA0 UART4_TX UART4 ALT(4)
|
||||||
|
PA1 UART4_RX UART4 ALT(4)
|
||||||
|
|
||||||
|
PA13 JTMS-SWDIO SWD
|
||||||
|
PA14 JTCK-SWCLK SWD
|
||||||
|
|
||||||
|
PA15 OSD_CS CS
|
||||||
|
PB3 BMP280_CS CS
|
||||||
|
PC12 SPI3_MOSI SPI3
|
||||||
|
PC11 SPI3_MISO SPI3
|
||||||
|
PC10 SPI3_SCK SPI3
|
||||||
|
|
||||||
|
PB5 LED_BLUE OUTPUT LOW GPIO(1)
|
||||||
|
define HAL_GPIO_A_LED_PIN 1
|
||||||
|
|
||||||
|
#dummy assignment required to allow AP_NOTIFY to use board led
|
||||||
|
define HAL_GPIO_B_LED_PIN 2
|
||||||
|
|
||||||
|
# TIM3 is needed for bi-directional dshot
|
||||||
|
#PB4 TIM3_CH1 TIM3 GPIO(58) ALARM LOW
|
||||||
|
|
||||||
|
#use LED-STRIP output as general purpose GPIO
|
||||||
|
PB6 GP_GPIO OUTPUT LOW GPIO(70)
|
||||||
|
|
||||||
|
PA11 OTG_FS_DM OTG1
|
||||||
|
PA12 OTG_FS_DP OTG1
|
||||||
|
PC5 VBUS INPUT OPENDRAIN
|
||||||
|
|
||||||
|
# "PPM" solder pad/resistor should be soldered and "S-BUS" resistor/solder pad removed
|
||||||
|
# Overwise UART1 or UART6 or RCIN will not work
|
||||||
|
PB8 TIM4_CH3 TIM4 RCININT PULLDOWN LOW
|
||||||
|
|
||||||
|
|
||||||
|
# SPI Device table
|
||||||
|
SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ
|
||||||
|
SPIDEV sdcard SPI2 DEVID2 SDCARD_CS MODE0 400*KHZ 25*MHZ
|
||||||
|
SPIDEV bmp280 SPI3 DEVID3 BMP280_CS MODE3 1*MHZ 8*MHZ
|
||||||
|
SPIDEV osd SPI3 DEVID4 OSD_CS MODE0 10*MHZ 10*MHZ
|
||||||
|
|
||||||
|
# one IMU
|
||||||
|
IMU Invensense SPI:mpu6000 ROTATION_YAW_180
|
||||||
|
|
||||||
|
# one baro
|
||||||
|
BARO BMP280 SPI:bmp280
|
||||||
|
|
||||||
|
# no built-in compass, but probe the i2c bus for all possible
|
||||||
|
# external compass types
|
||||||
|
define ALLOW_ARM_NO_COMPASS
|
||||||
|
define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
|
||||||
|
define HAL_PROBE_EXTERNAL_I2C_COMPASSES
|
||||||
|
define HAL_I2C_INTERNAL_MASK 0
|
||||||
|
define HAL_COMPASS_AUTO_ROT_DEFAULT 2
|
||||||
|
|
||||||
|
define STORAGE_FLASH_PAGE 1
|
||||||
|
define HAL_STORAGE_SIZE 15360
|
||||||
|
|
||||||
|
define HAL_OS_FATFS_IO 1
|
||||||
|
define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS"
|
||||||
|
define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN"
|
||||||
|
|
||||||
|
# define default battery setup
|
||||||
|
define HAL_BATT_VOLT_PIN 12
|
||||||
|
define HAL_BATT_CURR_PIN 11
|
||||||
|
define HAL_BATT_VOLT_SCALE 11
|
||||||
|
define HAL_BATT_CURR_SCALE 18.2
|
||||||
|
|
||||||
|
define OSD_ENABLED 1
|
||||||
|
define HAL_OSD_TYPE_DEFAULT 1
|
||||||
|
|
||||||
|
#To complementary channels work we define this
|
||||||
|
define STM32_PWM_USE_ADVANCED TRUE
|
||||||
|
|
||||||
|
define BOARD_PWM_COUNT_DEFAULT 8
|
||||||
|
|
||||||
|
#font for the osd
|
||||||
|
ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
|
||||||
|
|
||||||
|
# disable SMBUS and fuel battery monitors to save flash
|
||||||
|
define HAL_BATTMON_SMBUS_ENABLE 0
|
||||||
|
define HAL_BATTMON_FUEL_ENABLE 0
|
||||||
|
|
||||||
|
# disable parachute and sprayer to save flash
|
||||||
|
define HAL_PARACHUTE_ENABLED 0
|
||||||
|
define HAL_SPRAYER_ENABLED 0
|
||||||
|
|
||||||
|
# reduce max size of embedded params for apj_tool.py
|
||||||
|
define AP_PARAM_MAX_EMBEDDED_PARAM 1024
|
@ -1362,8 +1362,10 @@ def write_PWM_config(f):
|
|||||||
rc_in = None
|
rc_in = None
|
||||||
rc_in_int = None
|
rc_in_int = None
|
||||||
alarm = None
|
alarm = None
|
||||||
|
bidir = None
|
||||||
pwm_out = []
|
pwm_out = []
|
||||||
pwm_timers = []
|
pwm_timers = []
|
||||||
|
has_bidir = False
|
||||||
for l in bylabel.keys():
|
for l in bylabel.keys():
|
||||||
p = bylabel[l]
|
p = bylabel[l]
|
||||||
if p.type.startswith('TIM'):
|
if p.type.startswith('TIM'):
|
||||||
@ -1376,6 +1378,8 @@ def write_PWM_config(f):
|
|||||||
else:
|
else:
|
||||||
if p.extra_value('PWM', type=int) is not None:
|
if p.extra_value('PWM', type=int) is not None:
|
||||||
pwm_out.append(p)
|
pwm_out.append(p)
|
||||||
|
if p.has_extra('BIDIR'):
|
||||||
|
bidir = p
|
||||||
if p.type not in pwm_timers:
|
if p.type not in pwm_timers:
|
||||||
pwm_timers.append(p.type)
|
pwm_timers.append(p.type)
|
||||||
|
|
||||||
@ -1460,6 +1464,8 @@ def write_PWM_config(f):
|
|||||||
f.write('\n')
|
f.write('\n')
|
||||||
|
|
||||||
f.write('// PWM timer config\n')
|
f.write('// PWM timer config\n')
|
||||||
|
if bidir is not None:
|
||||||
|
f.write('#define HAL_WITH_BIDIR_DSHOT\n')
|
||||||
for t in sorted(pwm_timers):
|
for t in sorted(pwm_timers):
|
||||||
n = int(t[3:])
|
n = int(t[3:])
|
||||||
f.write('#define STM32_PWM_USE_TIM%u TRUE\n' % n)
|
f.write('#define STM32_PWM_USE_TIM%u TRUE\n' % n)
|
||||||
@ -1499,11 +1505,27 @@ def write_PWM_config(f):
|
|||||||
advanced_timer = 'false'
|
advanced_timer = 'false'
|
||||||
pwm_clock = 1000000
|
pwm_clock = 1000000
|
||||||
period = 20000 * pwm_clock / 1000000
|
period = 20000 * pwm_clock / 1000000
|
||||||
|
hal_icu_def = ''
|
||||||
|
hal_icu_cfg = ''
|
||||||
|
if bidir is not None:
|
||||||
|
hal_icu_cfg = '\n {'
|
||||||
|
hal_icu_def = '\n'
|
||||||
|
for i in range(1,5):
|
||||||
|
hal_icu_cfg += '{HAL_IC%u_CH%u_DMA_CONFIG},' % (n, i)
|
||||||
|
hal_icu_def +='''#if defined(STM32_TIM_TIM%u_CH%u_DMA_STREAM) && defined(STM32_TIM_TIM%u_CH%u_DMA_CHAN)
|
||||||
|
# define HAL_IC%u_CH%u_DMA_CONFIG true, STM32_TIM_TIM%u_CH%u_DMA_STREAM, STM32_TIM_TIM%u_CH%u_DMA_CHAN
|
||||||
|
#else
|
||||||
|
# define HAL_IC%u_CH%u_DMA_CONFIG false, 0, 0
|
||||||
|
#endif
|
||||||
|
''' % (n, i, n, i, n, i, n, i, n, i, n, i)
|
||||||
|
hal_icu_cfg += '}, \\'
|
||||||
|
|
||||||
|
|
||||||
f.write('''#if defined(STM32_TIM_TIM%u_UP_DMA_STREAM) && defined(STM32_TIM_TIM%u_UP_DMA_CHAN)
|
f.write('''#if defined(STM32_TIM_TIM%u_UP_DMA_STREAM) && defined(STM32_TIM_TIM%u_UP_DMA_CHAN)
|
||||||
# define HAL_PWM%u_DMA_CONFIG true, STM32_TIM_TIM%u_UP_DMA_STREAM, STM32_TIM_TIM%u_UP_DMA_CHAN
|
# define HAL_PWM%u_DMA_CONFIG true, STM32_TIM_TIM%u_UP_DMA_STREAM, STM32_TIM_TIM%u_UP_DMA_CHAN
|
||||||
#else
|
#else
|
||||||
# define HAL_PWM%u_DMA_CONFIG false, 0, 0
|
# define HAL_PWM%u_DMA_CONFIG false, 0, 0
|
||||||
#endif\n''' % (n, n, n, n, n, n))
|
#endif\n%s''' % (n, n, n, n, n, n, hal_icu_def))
|
||||||
f.write('''#define HAL_PWM_GROUP%u { %s, \\
|
f.write('''#define HAL_PWM_GROUP%u { %s, \\
|
||||||
{%u, %u, %u, %u}, \\
|
{%u, %u, %u, %u}, \\
|
||||||
/* Group Initial Config */ \\
|
/* Group Initial Config */ \\
|
||||||
@ -1518,14 +1540,14 @@ def write_PWM_config(f):
|
|||||||
{%s, NULL}, \\
|
{%s, NULL}, \\
|
||||||
{%s, NULL} \\
|
{%s, NULL} \\
|
||||||
}, 0, 0}, &PWMD%u, \\
|
}, 0, 0}, &PWMD%u, \\
|
||||||
HAL_PWM%u_DMA_CONFIG, \\
|
HAL_PWM%u_DMA_CONFIG, \\%s
|
||||||
{ %u, %u, %u, %u }, \\
|
{ %u, %u, %u, %u }, \\
|
||||||
{ %s, %s, %s, %s }}\n''' %
|
{ %s, %s, %s, %s }}\n''' %
|
||||||
(group, advanced_timer,
|
(group, advanced_timer,
|
||||||
chan_list[0], chan_list[1], chan_list[2], chan_list[3],
|
chan_list[0], chan_list[1], chan_list[2], chan_list[3],
|
||||||
pwm_clock, period,
|
pwm_clock, period,
|
||||||
chan_mode[0], chan_mode[1], chan_mode[2], chan_mode[3],
|
chan_mode[0], chan_mode[1], chan_mode[2], chan_mode[3],
|
||||||
n, n,
|
n, n, hal_icu_cfg,
|
||||||
alt_functions[0], alt_functions[1], alt_functions[2], alt_functions[3],
|
alt_functions[0], alt_functions[1], alt_functions[2], alt_functions[3],
|
||||||
pal_lines[0], pal_lines[1], pal_lines[2], pal_lines[3]))
|
pal_lines[0], pal_lines[1], pal_lines[2], pal_lines[3]))
|
||||||
f.write('#define HAL_PWM_GROUPS %s\n\n' % ','.join(groups))
|
f.write('#define HAL_PWM_GROUPS %s\n\n' % ','.join(groups))
|
||||||
@ -1859,6 +1881,9 @@ def build_peripheral_list():
|
|||||||
periph_pins.append(altmap[alt][p])
|
periph_pins.append(altmap[alt][p])
|
||||||
for p in periph_pins:
|
for p in periph_pins:
|
||||||
type = p.type
|
type = p.type
|
||||||
|
if type.startswith('TIM'):
|
||||||
|
# we need to independently demand DMA for each channel
|
||||||
|
type = p.label
|
||||||
if type in done:
|
if type in done:
|
||||||
continue
|
continue
|
||||||
for prefix in prefixes:
|
for prefix in prefixes:
|
||||||
@ -1889,9 +1914,13 @@ def build_peripheral_list():
|
|||||||
peripherals.append(label)
|
peripherals.append(label)
|
||||||
elif not p.has_extra('ALARM') and not p.has_extra('RCININT'):
|
elif not p.has_extra('ALARM') and not p.has_extra('RCININT'):
|
||||||
# get the TIMn_UP DMA channels for DShot
|
# get the TIMn_UP DMA channels for DShot
|
||||||
label = type + '_UP'
|
label = p.type + '_UP'
|
||||||
if label not in peripherals and not p.has_extra('NODMA'):
|
if label not in peripherals and not p.has_extra('NODMA'):
|
||||||
peripherals.append(label)
|
peripherals.append(label)
|
||||||
|
ch_label = type
|
||||||
|
(_, _, compl) = parse_timer(ch_label)
|
||||||
|
if ch_label not in peripherals and p.has_extra('BIDIR') and not compl:
|
||||||
|
peripherals.append(ch_label)
|
||||||
done.add(type)
|
done.add(type)
|
||||||
return peripherals
|
return peripherals
|
||||||
|
|
||||||
|
@ -4,7 +4,7 @@ import sys, fnmatch
|
|||||||
import importlib
|
import importlib
|
||||||
|
|
||||||
# peripheral types that can be shared, wildcard patterns
|
# peripheral types that can be shared, wildcard patterns
|
||||||
SHARED_MAP = ["I2C*", "USART*_TX", "UART*_TX", "SPI*", "TIM*_UP"]
|
SHARED_MAP = ["I2C*", "USART*_TX", "UART*_TX", "SPI*", "TIM*_UP", "TIM*_CH*"]
|
||||||
|
|
||||||
ignore_list = []
|
ignore_list = []
|
||||||
dma_map = None
|
dma_map = None
|
||||||
@ -163,18 +163,36 @@ def generate_DMAMUX_map_mask(peripheral_list, channel_mask, noshare_list, dma_ex
|
|||||||
continue
|
continue
|
||||||
|
|
||||||
# prevent attempts to share with other half of same peripheral
|
# prevent attempts to share with other half of same peripheral
|
||||||
|
# also prevent sharing with Timer channels
|
||||||
|
others = []
|
||||||
if p.endswith('RX'):
|
if p.endswith('RX'):
|
||||||
other = p[:-2] + 'TX'
|
others.append(p[:-2] + 'TX')
|
||||||
elif p.endswith('TX'):
|
elif p.endswith('TX'):
|
||||||
other = p[:-2] + 'RX'
|
others.append(p[:-2] + 'RX')
|
||||||
else:
|
|
||||||
other = None
|
|
||||||
|
|
||||||
if other is not None and ii in idsets[other]:
|
for p2 in peripheral_list:
|
||||||
if len(idsets[p]) >= len(idsets[other]) and len(idsets[other]) > 0:
|
if "_CH" not in p2:
|
||||||
continue
|
continue
|
||||||
idsets[other].remove(ii)
|
if "_UP" in p2 and "_CH" in p and p2[:4] == p[:4]:
|
||||||
dma_map[other].remove((dma,stream))
|
continue
|
||||||
|
elif "_UP" in p and "_CH" in p2 and p[:4] == p2[:4]:
|
||||||
|
continue
|
||||||
|
elif "_CH" in p and "_CH" in p2 and p[:4] == p2[:4]:
|
||||||
|
continue
|
||||||
|
else:
|
||||||
|
others.append(p2)
|
||||||
|
if debug:
|
||||||
|
print ("Others for ", p, others)
|
||||||
|
skip_this_chan = False
|
||||||
|
for other in others:
|
||||||
|
if ii in idsets[other]:
|
||||||
|
if len(idsets[p]) >= len(idsets[other]) or len(idsets[other]) <= 1:
|
||||||
|
skip_this_chan = True
|
||||||
|
break
|
||||||
|
idsets[other].remove(ii)
|
||||||
|
dma_map[other].remove((dma,stream))
|
||||||
|
if skip_this_chan:
|
||||||
|
continue
|
||||||
found = ii
|
found = ii
|
||||||
break
|
break
|
||||||
if found is None:
|
if found is None:
|
||||||
@ -292,6 +310,18 @@ def write_dma_header(f, peripheral_list, mcu_type, dma_exclude=[],
|
|||||||
stream = (streamchan[0], streamchan[1])
|
stream = (streamchan[0], streamchan[1])
|
||||||
share_ok = True
|
share_ok = True
|
||||||
for periph2 in stream_assign[stream]:
|
for periph2 in stream_assign[stream]:
|
||||||
|
# can only share timer UP and CH streams on the same timer, everything else disallowed
|
||||||
|
if "_CH" in periph or "_CH" in periph2:
|
||||||
|
if "_UP" in periph and "_CH" in periph2 and periph[-4:1] == periph2[-5:1]:
|
||||||
|
share_ok = True
|
||||||
|
elif "_UP" in periph2 and "_CH" in periph and periph2[-4:1] == periph[-5:1]:
|
||||||
|
share_ok = True
|
||||||
|
elif "_CH" in periph2 and "_CH" in periph and periph2[-5:1] == periph[-5:1]:
|
||||||
|
share_ok = True
|
||||||
|
else:
|
||||||
|
share_ok = False
|
||||||
|
if debug:
|
||||||
|
print ("Can't share ", periph, periph2)
|
||||||
if not can_share(periph, noshare_list) or not can_share(periph2, noshare_list):
|
if not can_share(periph, noshare_list) or not can_share(periph2, noshare_list):
|
||||||
share_ok = False
|
share_ok = False
|
||||||
if share_ok:
|
if share_ok:
|
||||||
|
Loading…
Reference in New Issue
Block a user