static int es3_tsb_unipro_set_init_status(uint32_t val)
{
int retval;
uint32_t status;
/*
* See SW-1228
* -> https://projectara.atlassian.net/browse/SW-1228?focusedCommentId=28889&page=com.atlassian.jira.plugin.system.issuetabpanels:comment-tabpanel#comment-28889
*/
if (val & INIT_STATUS_ERROR_CODE_MASK) {
lowsyslog("%s() cannot override the init status error code (init status: %u).\n",
__func__, val);
return -EINVAL;
}
retval = unipro_attr_local_read(TSB_DME_ES3_INIT_STATUS, &status,
UNIPRO_SELINDEX_NULL);
if (retval) {
lowsyslog("init-status read failed: retval = %d\n", retval);
return retval;
}
/* preserve error code: see SW-1228 */
val |= status & INIT_STATUS_ERROR_CODE_MASK;
retval = unipro_attr_local_write(TSB_DME_ES3_INIT_STATUS, val,
UNIPRO_SELINDEX_NULL);
if (retval) {
lowsyslog("init-status write failed: retval = %d\n", retval);
return retval;
}
return 0;
}
static void pwm_enable(void)
{
struct pwm_lowerhalf_s *pwm;
struct pwm_info_s pwm_info;
int ret;
int fd;
pwm_info.frequency = 300000;
pwm_info.duty = 32767;
pwm = tsb_pwminitialize(0);
if (!pwm) {
lowsyslog("pwminitialize failed\n");
return;
}
if (pwm_register("/dev/pwm0", pwm)) {
lowsyslog("Can't register /dev/pwm0\n");
return;
}
fd = open("/dev/pwm0", O_RDONLY);
ret = ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)&pwm_info);
if (ret < 0) {
lowsyslog("pwm_main: ioctl(PWMIOC_SETCHARACTERISTICS) failed: %d\n",
errno);
}
ret = ioctl(fd, PWMIOC_START, 0);
if (ret < 0) {
lowsyslog("pwm_main: ioctl(PWMIOC_START) failed: %d\n", errno);
}
}
static void
warnerr_core(int errcode, const char *fmt, va_list args)
{
#if CONFIG_NFILE_STREAMS > 0
fprintf(stderr, "%s: ", getprogname());
vfprintf(stderr, fmt, args);
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
errcode = -errcode;
if (errcode < NOCODE)
fprintf(stderr, ": %s", strerror(errcode));
fprintf(stderr, "\n");
#elif CONFIG_ARCH_LOWPUTC
lowsyslog("%s: ", getprogname());
lowvyslog(fmt, args);
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
errcode = -errcode;
if (errcode < NOCODE)
lowsyslog(": %s", strerror(errcode));
lowsyslog("\n");
#endif
}
static int io_expander_init(void)
{
void *driver_data;
struct i2c_dev_s *dev;
dev = up_i2cinitialize(0);
if (!dev) {
lowsyslog("%s(): Failed to get I/O Expander I2C bus 0\n", __func__);
return -ENODEV;
} else {
if (tca64xx_init(&driver_data,
TCA6408_PART,
dev,
TCA6408_U72,
TCA6408_U72_RST_GPIO,
TCA6408_U72_INT_GPIO,
TCA6408_GPIO_BASE) < 0) {
lowsyslog("%s(): Failed to register I/O Expander(0x%02x)\n",
__func__, TCA6408_U72);
up_i2cuninitialize(dev);
return -ENODEV;
}
}
return 0;
}
static int print_param(const char* name, bool verbose)
{
static int _max_name_len;
if (_max_name_len == 0) {
for (int i = 0;; i++) {
const char* nm = config_name_by_index(i);
if (!nm)
break;
int len = strlen(nm);
if (len > _max_name_len)
_max_name_len = len;
}
}
struct config_param par;
const int res = config_get_descr(name, &par);
if (res)
return res;
if (par.type == CONFIG_TYPE_FLOAT) {
lowsyslog("%-*s = %-12f", _max_name_len, name, config_get(name));
if (verbose) {
lowsyslog("[%f, %f] (%f)", par.min, par.max, par.default_);
}
} else {
lowsyslog("%-*s = %-12i", _max_name_len, name, (int)config_get(name));
if (verbose) {
lowsyslog("[%i, %i] (%i)", (int)par.min, (int)par.max, (int)par.default_);
}
}
puts("");
return 0;
}
static int test_sensors(void)
{
/*
* Enable the synchronous PWM on all phases, obtain a sample and disable PWM again
*/
const enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
MOTOR_PWM_MANIP_HALF,
MOTOR_PWM_MANIP_HALF,
MOTOR_PWM_MANIP_HALF
};
motor_pwm_manip(manip_cmd);
usleep(INITIAL_DELAY_MS * 1000);
const struct motor_adc_sample sample = motor_adc_get_last_sample();
motor_pwm_set_freewheeling();
/*
* Validate the obtained sample
*/
const bool valid_voltage = (sample.input_voltage_raw > 0) && (sample.input_voltage_raw < MOTOR_ADC_SAMPLE_MAX);
const bool valid_temperature = (sample.temperature_raw > 0) && (sample.temperature_raw < MOTOR_ADC_SAMPLE_MAX);
if (!valid_voltage || !valid_temperature) {
lowsyslog("Motor: Invalid sensor readings: raw input voltage %i, raw temperature %i\n",
sample.input_voltage_raw, sample.temperature_raw);
return 1;
}
lowsyslog("Motor: Raw phase voltages: %i %i %i\n",
sample.phase_voltage_raw[0], sample.phase_voltage_raw[1], sample.phase_voltage_raw[2]);
lowsyslog("Motor: Raw phase currents: %i %i\n", sample.phase_current_raw[0], sample.phase_current_raw[1]);
lowsyslog("Motor: Raw input voltage, temperature: %i %i\n", sample.input_voltage_raw, sample.temperature_raw);
return 0;
}
int motor_rtctl_test_hardware(void)
{
if (motor_rtctl_get_state() != MOTOR_RTCTL_STATE_IDLE) {
return -1;
}
motor_pwm_set_freewheeling();
usleep(INITIAL_DELAY_MS * 1000);
lowsyslog("Motor: Power stage test...\n");
{
int res = test_power_stage();
if (res != 0) {
return res;
}
}
lowsyslog("Motor: Cross phase test...\n");
{
int res = test_cross_phase_conductivity();
if (res != 0) {
return res;
}
}
lowsyslog("Motor: Sensors test...\n");
{
int res = test_sensors();
if (res != 0) {
return res;
}
}
return 0;
}
int
PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case 1: /* XXX magic number - test operation */
switch (arg) {
case 0:
lowsyslog("test 0\n");
/* kill DMA, this is a PIO test */
stm32_dmastop(_tx_dma);
stm32_dmastop(_rx_dma);
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
for (;;) {
while (!(rSR & USART_SR_TXE))
;
rDR = 0x55;
}
return 0;
case 1:
{
unsigned fails = 0;
for (unsigned count = 0;; count++) {
uint16_t value = count & 0xffff;
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
fails++;
if (count >= 5000) {
lowsyslog("==== test 1 : %u failures ====\n", fails);
perf_print_counter(_pc_txns);
perf_print_counter(_pc_dmasetup);
perf_print_counter(_pc_retries);
perf_print_counter(_pc_timeouts);
perf_print_counter(_pc_crcerrs);
perf_print_counter(_pc_dmaerrs);
perf_print_counter(_pc_protoerrs);
perf_print_counter(_pc_uerrs);
perf_print_counter(_pc_idle);
perf_print_counter(_pc_badidle);
count = 0;
}
}
return 0;
}
case 2:
lowsyslog("test 2\n");
return 0;
}
default:
break;
}
return -1;
}
static int init_constants(unsigned frequency)
{
assert_always(_pwm_top == 0); // Make sure it was not initialized already
/*
* PWM top, derived from frequency
*/
if (frequency < MOTOR_PWM_MIN_FREQUENCY || frequency > MOTOR_PWM_MAX_FREQUENCY) {
return -1;
}
const int pwm_steps = PWM_TIMER_FREQUENCY / frequency;
if ((pwm_steps < 2) || (pwm_steps > 65536)) {
assert(0);
return -1;
}
_pwm_top = pwm_steps - 1;
_pwm_half_top = pwm_steps / 2;
const int effective_steps = pwm_steps / 2;
const float true_frequency = PWM_TIMER_FREQUENCY / (float)pwm_steps;
const unsigned adc_period_usec = motor_adc_sampling_period_hnsec() / HNSEC_PER_USEC;
lowsyslog("Motor: PWM freq: %f; Effective steps: %i; ADC period: %u usec\n",
true_frequency, effective_steps, adc_period_usec);
/*
* PWM min - Limited by MOTOR_ADC_SAMPLE_WINDOW_NANOSEC
*/
const float pwm_clock_period = 1.f / PWM_TIMER_FREQUENCY;
const float pwm_adc_window_len = (MOTOR_ADC_SAMPLE_WINDOW_NANOSEC / 1e9f) * 2;
const float pwm_adc_window_ticks_float = pwm_adc_window_len / pwm_clock_period;
assert(pwm_adc_window_ticks_float >= 0);
uint16_t pwm_half_adc_window_ticks = (uint16_t)pwm_adc_window_ticks_float;
if (pwm_half_adc_window_ticks > _pwm_half_top) {
pwm_half_adc_window_ticks = _pwm_half_top;
}
_pwm_min = pwm_half_adc_window_ticks;
assert(_pwm_min <= _pwm_half_top);
/*
* ADC synchronization.
* ADC shall be triggered in the middle of a PWM cycle in order to catch the moment when the instant
* winding current matches with average winding current - this helps to eliminate the current ripple
* caused by the PWM switching. Thus we trigger ADC at the point ((pwm_value / 2) - adc_advance).
* Refer to "Synchronizing the On-Chip Analog-to-Digital Converter on 56F80x Devices" for some explanation.
*/
const float adc_trigger_advance = MOTOR_ADC_SYNC_ADVANCE_NANOSEC / 1e9f;
const float adc_trigger_advance_ticks_float = adc_trigger_advance / pwm_clock_period;
assert(adc_trigger_advance_ticks_float >= 0);
assert(adc_trigger_advance_ticks_float < (_pwm_top * 0.4f));
_adc_advance_ticks = (uint16_t)adc_trigger_advance_ticks_float;
lowsyslog("Motor: PWM range [%u; %u], ADC advance ticks %u\n",
(unsigned)_pwm_min, (unsigned)_pwm_top, (unsigned)_adc_advance_ticks);
return 0;
}
static void cmd_stat(BaseSequentialStream *chp, int argc, char *argv[])
{
float voltage = 0, current = 0;
motor_get_input_voltage_current(&voltage, ¤t);
lowsyslog("Power V/A %-9f %f\n", voltage, current);
lowsyslog("RPM/DC %-9u %f\n", motor_get_rpm(), motor_get_duty_cycle());
lowsyslog("Active limits %i\n", motor_get_limit_mask());
lowsyslog("ZC failures %lu\n", (unsigned long)motor_get_zc_failures_since_start());
}
void motor_rtctl_start(float duty_cycle, bool reverse)
{
motor_rtctl_stop(); // Just in case
if (!_initialization_confirmed) {
return; // Go home you're drunk
}
if (duty_cycle <= 0) {
assert(0);
return;
}
/*
* Initialize the structs
*/
memset(&_diag, 0, sizeof(_diag));
memset(&_state, 0, sizeof(_state)); // Mighty reset
motor_forced_rotation_detector_reset();
_diag.started_at = motor_timer_hnsec();
_state.comm_table = reverse ? COMMUTATION_TABLE_REVERSE : COMMUTATION_TABLE_FORWARD;
_state.pwm_val_after_spinup = motor_pwm_compute_pwm_val(duty_cycle);
init_adc_filters();
/*
* Initial spinup.
*/
const tprio_t orig_priority = chThdSetPriority(HIGHPRIO);
motor_pwm_prepare_to_start();
const bool started = do_bemf_spinup(duty_cycle);
/*
* Engage the normal mode if started
* At this point, if the spinup was OK, we're sutiated RIGHT AFTER THE DETECTED ZERO CROSS, engaged.
*/
if (started) {
_state.blank_time_deadline = motor_timer_hnsec() + _params.comm_blank_hnsec;
_state.zc_detection_result = ZC_DETECTED;
_state.flags = FLAG_ACTIVE | FLAG_SPINUP;
motor_timer_set_relative(_state.comm_period / 3);
lowsyslog("Motor: Spinup OK, comm period: %u usec\n", (unsigned)(_state.comm_period / HNSEC_PER_USEC));
} else {
lowsyslog("Motor: Spinup failed\n");
motor_rtctl_stop();
}
chThdSetPriority(orig_priority);
}
int _unipro_reset_cport(unsigned int cportid)
{
int rc;
int retval = 0;
uint32_t tx_reset_offset;
uint32_t rx_reset_offset;
uint32_t tx_queue_empty_offset;
unsigned tx_queue_empty_bit;
if (cportid >= cport_count) {
return -EINVAL;
}
tx_queue_empty_offset = CPB_TXQUEUEEMPTY_0 + ((cportid / 32) << 2);
tx_queue_empty_bit = (1 << (cportid % 32));
while (!(getreg32(AIO_UNIPRO_BASE + tx_queue_empty_offset) &
tx_queue_empty_bit)) {
}
tx_reset_offset = TX_SW_RESET_00 + (cportid << 2);
rx_reset_offset = RX_SW_RESET_00 + (cportid << 2);
putreg32(CPORT_SW_RESET_BITS, AIO_UNIPRO_BASE + tx_reset_offset);
rc = unipro_attr_local_write(T_CONNECTIONSTATE, 0, cportid);
if (rc) {
lowsyslog("error resetting T_CONNECTIONSTATE (%d)\n", rc);
retval = -EIO;
}
rc = unipro_attr_local_write(T_LOCALBUFFERSPACE, 0, cportid);
if (rc) {
lowsyslog("error resetting T_LOCALBUFFERSPACE (%d)\n", rc);
retval = -EIO;
}
rc = unipro_attr_local_write(T_PEERBUFFERSPACE, 0, cportid);
if (rc) {
lowsyslog("error resetting T_PEERBUFFERSPACE (%d)\n", rc);
retval = -EIO;
}
rc = unipro_attr_local_write(T_CREDITSTOSEND, 0, cportid);
if (rc) {
lowsyslog("error resetting T_CREDITSTOSEND (%d)\n", rc);
retval = -EIO;
}
putreg32(CPORT_SW_RESET_BITS, AIO_UNIPRO_BASE + rx_reset_offset);
putreg32(0, AIO_UNIPRO_BASE + tx_reset_offset);
putreg32(0, AIO_UNIPRO_BASE + rx_reset_offset);
return retval;
}
/**
* Sets high/low levels on the output FETs, reads ADC samples making sure that there are proper corellations.
*/
static int test_power_stage(void)
{
int result = 0;
int high_samples[MOTOR_NUM_PHASES];
memset(high_samples, 0, sizeof(high_samples));
const int threshold = ((1 << MOTOR_ADC_RESOLUTION) * ANALOG_TOLERANCE_PERCENT) / 100;
motor_pwm_set_freewheeling();
/*
* Test phases at low level; collect high level readings
*/
for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
// Low level
const int low = test_one_phase(phase, false);
if (low > threshold) {
lowsyslog("Motor: Selftest FAILURE at phase %i: low sample %i is above threshold %i\n",
phase, low, threshold);
result++;
}
// High level
const int high = test_one_phase(phase, true);
high_samples[phase] = high;
if (high < threshold) {
lowsyslog("Motor: Selftest FAILURE at phase %i: high sample %i is below threshold %i\n",
phase, high, threshold);
result++;
}
// It is not possible to check against the high threshold directly
// because its value will depend on the supply voltage
lowsyslog("Motor: Selftest phase %i: low %i, high %i\n", phase, low, high);
}
/*
* Make sure that the high level readings are nearly identical
*/
int high_samples_sorted[MOTOR_NUM_PHASES];
memcpy(high_samples_sorted, high_samples, sizeof(high_samples));
qsort(high_samples_sorted, MOTOR_NUM_PHASES, sizeof(int), compare_samples);
const int high_median = high_samples_sorted[MOTOR_NUM_PHASES / 2];
for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
if (abs(high_samples[phase] - high_median) > threshold) {
lowsyslog("Motor: Selftest FAILURE at phase %i: sample %i is far from median %i\n",
phase, high_samples[phase], high_median);
result++;
}
}
motor_pwm_set_freewheeling();
return result;
}
void _gb_dump(const char *func, __u8 *buf, size_t size)
{
int i;
irqstate_t flags;
flags = irqsave();
lowsyslog("%s:\n", func);
for (i = 0; i < size; i++) {
lowsyslog( "%02x ", buf[i]);
}
lowsyslog("\n");
irqrestore(flags);
}
/**
* @brief Initialize SPI controller
*
* @param info pointer to the tsb_spi_dev_info struct.
* @return 0 on success, negative errno on error
*/
static int tsb_spi_hw_init(struct tsb_spi_dev_info *info)
{
uint32_t ctrl0;
int ret = 0;
/* Enable clock */
tsb_clk_enable(TSB_CLK_SPIP);
tsb_clk_enable(TSB_CLK_SPIS);
/* Reset */
tsb_reset(TSB_RST_SPIP);
tsb_reset(TSB_RST_SPIS);
info->spi_pmstate = PM_NORMAL;
/* Disable SPI controller */
tsb_spi_write(info->reg_base, DW_SPI_SSIENR, 0);
/* Disable SPI all interrupts */
tsb_spi_write(info->reg_base, DW_SPI_IMR, 0);
/* Clear interrupt status register */
tsb_spi_read(info->reg_base, DW_SPI_ICR);
/* SPI supports both transmit and receiver mode */
ctrl0 = tsb_spi_read(info->reg_base, DW_SPI_CTRLR0);
ctrl0 &= ~SPI_CTRL0_TMOD_MASK;
tsb_spi_write(info->reg_base, DW_SPI_CTRLR0, ctrl0);
/* get SPIM_CLK, SPIM_SDI and SPIM_SDO */
ret = tsb_pin_request(PIN_SPIM);
if (ret) {
lowsyslog("SPI: cannot get ownership of PIN_SPIM\n");
goto err_req_pinshare;
}
#if !defined(CONFIG_TSB_SPI_GPIO)
/* get also SPIM_CS0_N and SPIM_CS1_N */
ret = tsb_pin_request(PIN_SPIM_CSX_N);
if (ret) {
lowsyslog("SPI: cannot get ownership of PIN_SPIM_CSX_N\n");
goto err_req_pinshare_gpio;
}
#endif
return 0;
err_req_pinshare_gpio:
tsb_pin_release(PIN_SPIM);
err_req_pinshare:
return ret;
}
msg_t main() override
{
zubax_chibios::watchdog::Timer wdt;
wdt.startMSec(1000);
setName("mag");
::usleep(500000); // Startup delay
wdt.reset();
node::markComponentInitialized(node::ComponentID::Magnetometer);
while (!tryInit() && !node::hasPendingRestartRequest())
{
setStatus(uavcan::protocol::NodeStatus::HEALTH_ERROR);
lowsyslog("Mag init failed, will retry...\n");
::usleep(500000);
wdt.reset();
}
wdt.reset();
const float variance = param_variance.get();
const uint64_t period_usec = param_period_usec.get();
systime_t sleep_until = chibios_rt::System::getTime();
while (!node::hasPendingRestartRequest())
{
sleep_until += US2ST(period_usec);
float vector[3] = {0, 0, 0};
if (tryRead(vector))
{
transformToNEDFrame(vector);
publish(vector, variance);
setStatus(estimateStatusFromMeasurement(vector));
}
else
{
setStatus(uavcan::protocol::NodeStatus::HEALTH_ERROR);
}
sysSleepUntilChTime(sleep_until);
wdt.reset();
}
lowsyslog("Mag driver terminated\n");
return msg_t();
}
void z16f_sysexec(FAR chipreg_t *regs)
{
uint16_t excp;
/* Save that register reference so that it can be used for built-in
* diagnostics.
*/
g_current_regs = regs;
/* The cause of the system exception is indicated in the SYSEXCPH&L
* registers
*/
excp = getreg16(Z16F_SYSEXCP);
if ((excp & Z16F_SYSEXCP_SPOVF) != 0)
{
lowsyslog(LOG_ERR, "SP OVERFLOW\n");
}
if ((excp & Z16F_SYSEXCP_PCOVF) != 0)
{
lowsyslog(LOG_ERR, "PC OVERFLOW\n");
}
if ((excp & Z16F_SYSEXCP_DIV0) != 0)
{
lowsyslog(LOG_ERR, "Divide by zero\n");
}
if ((excp & Z16F_SYSEXCP_DIVOVF) != 0)
{
lowsyslog(LOG_ERR, "Divide overflow\n");
}
if ((excp & Z16F_SYSEXCP_ILL) != 0)
{
lowsyslog(LOG_ERR, "Illegal instruction\n");
}
if ((excp & Z16F_SYSEXCP_WDTOSC) != 0)
{
lowsyslog(LOG_ERR, "WDT oscillator failure\n");
}
if ((excp & Z16F_SYSEXCP_PRIOSC) != 0)
{
lowsyslog(LOG_ERR, "Primary Oscillator Failure\n");
}
if ((excp & Z16F_SYSEXCP_WDT) != 0)
{
lowsyslog(LOG_ERR, "Watchdog timeout\n");
z16f_reset();
}
PANIC();
}
int motor_init(void)
{
_watchdog_id = watchdog_create(WATCHDOG_TIMEOUT_MSEC);
if (_watchdog_id < 0) {
return _watchdog_id;
}
int ret = motor_rtctl_init();
if (ret) {
return ret;
}
chMtxInit(&_mutex);
chEvtInit(&_setpoint_update_event);
configure();
init_filters();
if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
lowsyslog("Motor: Invalid input voltage: %f\n", _state.input_voltage);
return -1;
}
ret = rpmctl_init();
if (ret) {
return ret;
}
motor_rtctl_stop();
assert_always(chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread),
HIGHPRIO, control_thread, NULL));
return 0;
}
static void configure(void)
{
_params.timing_advance_deg64 = config_get("motor_timing_advance_deg") * 64 / 60;
_params.motor_bemf_window_len_denom = config_get("motor_bemf_window_len_denom");
_params.bemf_valid_range_pct128 = config_get("motor_bemf_valid_range_pct") * 128 / 100;
_params.zc_failures_max = config_get("motor_zc_failures_to_stop");
_params.zc_detects_min = config_get("motor_zc_detects_to_start");
_params.comm_period_max = config_get("motor_comm_period_max_usec") * HNSEC_PER_USEC;
_params.comm_blank_hnsec = config_get("motor_comm_blank_usec") * HNSEC_PER_USEC;
_params.spinup_timeout = config_get("motor_spinup_timeout_ms") * HNSEC_PER_MSEC;
_params.spinup_start_comm_period = config_get("motor_spinup_start_comm_period_usec") * HNSEC_PER_USEC;
_params.spinup_end_comm_period = config_get("motor_spinup_end_comm_period_usec") * HNSEC_PER_USEC;
_params.spinup_num_good_comms = config_get("motor_spinup_num_good_comms");
_params.spinup_duty_cycle_increment = config_get("motor_spinup_duty_cycle_inc");
/*
* Validation
*/
if (_params.comm_period_max > motor_timer_get_max_delay_hnsec()) {
_params.comm_period_max = motor_timer_get_max_delay_hnsec();
}
if (_params.spinup_end_comm_period > _params.comm_period_max) {
_params.spinup_end_comm_period = _params.comm_period_max;
}
_params.adc_sampling_period = motor_adc_sampling_period_hnsec();
lowsyslog("Motor: RTCTL config: Max comm period: %u usec, BEMF window denom: %i\n",
(unsigned)(_params.comm_period_max / HNSEC_PER_USEC),
_params.motor_bemf_window_len_denom);
}
static void update_control_non_running(void)
{
// Do not change anything while the motor is starting
const enum motor_rtctl_state rtctl_state = motor_rtctl_get_state();
if (rtctl_state == MOTOR_RTCTL_STATE_STARTING) {
return;
}
// Start if necessary
const float spinup_dc = _params.dc_min_voltage / _state.input_voltage;
const bool need_start =
(_state.mode == MOTOR_CONTROL_MODE_OPENLOOP && (_state.dc_openloop_setpoint > 0)) ||
(_state.mode == MOTOR_CONTROL_MODE_RPM && (_state.rpm_setpoint > 0));
if (need_start && (_state.num_unexpected_stops < _params.num_unexpected_stops_to_latch)) {
const uint64_t timestamp = motor_rtctl_timestamp_hnsec();
_state.dc_actual = spinup_dc;
motor_rtctl_start(spinup_dc, _params.reverse);
_state.rtctl_state = motor_rtctl_get_state();
// This HACK prevents the setpoint TTL expiration in case of protracted startup
const int elapsed_ms = (motor_rtctl_timestamp_hnsec() - timestamp) / HNSEC_PER_MSEC;
_state.setpoint_ttl_ms += elapsed_ms;
lowsyslog("Motor: Startup %i ms, DC %f, mode %i\n", elapsed_ms, spinup_dc, _state.mode);
if (_state.rtctl_state == MOTOR_RTCTL_STATE_IDLE) {
handle_unexpected_stop();
}
}
}
void arm_gic_dump(const char *msg, bool all, int irq)
{
unsigned int nlines = arm_gic_nlines();
if (all)
{
lowsyslog(LOG_INFO, "GIC: %s NLINES=%u\n", msg, nlines);
}
else
{
lowsyslog(LOG_INFO, "GIC: %s IRQ=%d\n", msg, irq);
}
arm_gic_dump_cpu(all, irq, nlines);
arm_gic_dump_distributor(all, irq, nlines);
}
static void unipro_evt_handler(enum unipro_event evt)
{
int retval;
DBG_UNIPRO("UniPro: event %d.\n", evt);
switch (evt) {
case UNIPRO_EVT_MAILBOX:
mailbox_evt();
break;
case UNIPRO_EVT_LUP_DONE:
if (tsb_get_rev_id() == tsb_rev_es2)
es2_apply_mphy_fixup();
retval = unipro_enable_mailbox_irq();
if (retval) {
lowsyslog("unipro: failed to enable mailbox irq\n");
}
break;
}
if (evt_handler) {
evt_handler(evt);
}
}
static void print_status(int err)
{
if (err == 0)
puts("OK");
else
lowsyslog("ERROR %d %s\n", err, strerror(abs(err)));
}
void tcp_wrbuffer_dump(FAR const char *msg, FAR struct tcp_wrbuffer_s *wrb,
unsigned int len, unsigned int offset)
{
lowsyslog(LOG_DEBUG, "%s: wrb=%p segno=%d sent=%d nrtx=%d\n",
msg, wrb, WRB_SEQNO(wrb), WRB_SENT(wrb), WRB_NRTX(wrb));
iob_dump("I/O Buffer Chain", WRB_IOB(wrb), len, offset);
}
static void cmd_rpm(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 30000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" rpm <RPM>\n"
" rpm arm");
return;
}
// Safety check
static bool _armed = false;
if (!strcmp(argv[0], "arm")) {
_armed = true;
puts("OK");
return;
}
if (!_armed) {
puts("Error: Not armed");
return;
}
long value = (long)atoff(argv[0]);
value = (value < 0) ? 0 : value;
value = (value > 65535) ? 65535 : value;
lowsyslog("RPM %li\n", value);
motor_set_rpm((unsigned)value, TTL_MS);
}
static void cmd_dc(BaseSequentialStream *chp, int argc, char *argv[])
{
static const int TTL_MS = 30000;
if (argc == 0) {
motor_stop();
puts("Usage:\n"
" dc <duty cycle>\n"
" dc arm");
return;
}
// Safety check
static bool _armed = false;
if (!strcmp(argv[0], "arm")) {
_armed = true;
puts("OK");
return;
}
if (!_armed) {
puts("Error: Not armed");
return;
}
const float value = atoff(argv[0]);
lowsyslog("Duty cycle %f\n", value);
motor_set_duty_cycle(value, TTL_MS);
}
/**
* Detects cross-phase short circuit
*/
static int test_cross_phase_conductivity(void)
{
int num_detects = 0;
for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
// Apply the high level voltage to the selected phase
enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
MOTOR_PWM_MANIP_FLOATING,
MOTOR_PWM_MANIP_FLOATING,
MOTOR_PWM_MANIP_FLOATING
};
manip_cmd[phase] = MOTOR_PWM_MANIP_HIGH;
motor_pwm_set_freewheeling();
usleep(SAMPLE_DELAY_MS * 1000);
motor_pwm_manip(manip_cmd);
usleep(SAMPLE_DELAY_MS * 1000);
motor_pwm_set_freewheeling();
// Read back the full ADC sample
const struct motor_adc_sample sample = motor_adc_get_last_sample();
// Make sure only one phase is at the high level; other must be at low level
const int low_first = (phase + 1) % MOTOR_NUM_PHASES;
const int low_second = (phase + 2) % MOTOR_NUM_PHASES;
assert((low_first != phase) && (low_second != phase) && (low_first != low_second));
const int low_sum = sample.phase_voltage_raw[low_first] + sample.phase_voltage_raw[low_second];
const bool valid = (low_sum * 2) < sample.phase_voltage_raw[phase];
if (!valid) {
num_detects++;
lowsyslog("Motor: Phase %i cross conductivity: %i %i %i\n", phase,
sample.phase_voltage_raw[0], sample.phase_voltage_raw[1], sample.phase_voltage_raw[2]);
}
}
motor_pwm_set_freewheeling();
if (num_detects == MOTOR_NUM_PHASES) {
lowsyslog("Motor: All phases are shorted, assuming the motor is connected\n");
num_detects = 0;
}
return num_detects;
}
void up_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC();
#else
int index;
int irq;
/* Read the IRQ vector status register. Bits 3-10 provide the IRQ number
* of the interrupt (the TABLE_ADDR was initialized to zero, so the
* following masking should be unnecessary)
*/
index = getreg32(LPC31_INTC_VECTOR0) & INTC_VECTOR_INDEX_MASK;
if (index != 0)
{
/* Shift the index so that the range of IRQ numbers are in bits 0-7 (values
* 1-127) and back off the IRQ number by 1 so that the numbering is zero-based
*/
irq = (index >> INTC_VECTOR_INDEX_SHIFT) -1;
/* Verify that the resulting IRQ number is valid */
if ((unsigned)irq < NR_IRQS)
{
uint32_t* savestate;
/* Mask and acknowledge the interrupt */
up_maskack_irq(irq);
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still
* disabled).
*/
up_enable_irq(irq);
}
}
static void lpc214x_decodeirq( uint32_t *regs)
#endif
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
/* Decode the interrupt. We have to do this by search for the lowest numbered
* non-zero bit in the interrupt status register.
*/
uint32_t pending = vic_getreg(LPC214X_VIC_IRQSTATUS_OFFSET) & 0x007fffff;
unsigned int nibble;
unsigned int irq_base;
unsigned int irq = NR_IRQS;
/* Search in groups of four bits. For 22 sources, this is at most six
* times through the loop.
*/
for (nibble = pending & 0x0f, irq_base = 0;
pending && irq_base < NR_IRQS;
pending >>= 4, nibble = pending & 0x0f, irq_base += 4)
{
if (nibble)
{
irq = irq_base + g_nibblemap[nibble];
break;
}
}
/* Verify that the resulting IRQ number is valid */
if (irq < NR_IRQS)
{
uint32_t *savestate;
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
}
#endif
}
void up_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
board_led_on(LED_INIRQ);
lowsyslog("Unexpected IRQ\n");
current_regs = regs;
PANIC();
#else
unsigned int irq;
/* Read the IRQ number from the IVR register (Could probably get the same
* info from CIC register without the setup).
*/
board_led_on(LED_INIRQ);
irq = getreg32(STR71X_EIC_IVR);
/* Verify that the resulting IRQ number is valid */
if (irq < NR_IRQS)
{
uint32_t* savestate;
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Mask and acknowledge the interrupt */
up_maskack_irq(irq);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still disabled) */
up_enable_irq(irq);
}
#if CONFIG_DEBUG
else
{
PANIC(); /* Normally never happens */
}
#endif
board_led_off(LED_INIRQ);
#endif
}
