/**
* @brief RAM MBIST测试：
* DUT MBIST 模式。
* GS MCU 执行测试并将结果反馈给 ATE
* @param None.
* @retval Test result.
*/
u32 CTest::Test_MBIST(void)
{
TestResBusy();
m_dut.mbist.StartMBIST(); // 为保证时序正确，先发信号，再进入模式。
m_dut.mbist.Open();
// Test operation
if (m_dut.mbist.WaitForDone_WithTimeOut(1000))
TestResFail(); // time out
else if (m_dut.mbist.Result())
TestResFail();
else
TestResPass();
WaitForTestEnd();
m_dut.mbist.Close();
// restore gpio configuration for other use, eg. SPI2
CGPIO_Pin_Out_PP dut_wakeup(GPIOC, 4);
CSPI spi2(SPI2, NULL);
CRF dut_rf(spi2, NULL, BB_GPIO_OUT(GPIOC, 7), BB_GPIO_IN(GPIOC, 5));
TestResIdle();
return m_test_res;
}
void main_task(void *pvParameters)
{
(void) pvParameters;
vTaskDelay(500 / portTICK_RATE_MS);
SPIMaster spi5(SPI5, SPI_BAUDRATEPRESCALER_32, 0x2000, {
{MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
{MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
{MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
}, {
{GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{ACCEL_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
});
SPIMaster spi2(SPI2, SPI_BAUDRATEPRESCALER_32, 0x2000, {
{EXT_MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
{EXT_MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
{EXT_MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
}, {
{EXT_GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{LPS25HB_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{BMP280_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
});
L3GD20 gyro(spi5, 0);
LPS25HB lps25hb(spi2, 1);
BMP280 bmp2(spi2, 2);
LSM303D accel(spi5, 1);
uint8_t gyro_wtm = 5;
uint8_t acc_wtm = 8;
TimeStamp console_update_time;
TimeStamp sample_dt;
TimeStamp led_toggle_ts;
FlightControl flight_ctl;
static bool print_to_console = false;
LowPassFilter<Vector3f, float> gyro_lpf({0.5});
LowPassFilter<Vector3f, float> acc_lpf_alt({0.9});
LowPassFilter<Vector3f, float> acc_lpf_att({0.990});
LowPassFilter<float, float> pressure_lpf({0.6});
attitudetracker att;
/*
* Apply the boot configuration from flash memory.
*/
dronestate_boot_config(*drone_state);
L3GD20Reader gyro_reader(gyro, GYRO_INT2_PIN, gyro_align);
LSM303Reader acc_reader(accel, ACC_INT2_PIN, acc_align);
UartRpcServer rpcserver(*drone_state, configdata, acc_reader.mag_calibrator_);
bmp2.set_oversamp_pressure(BMP280_OVERSAMP_16X);
bmp2.set_work_mode(BMP280_ULTRA_HIGH_RESOLUTION_MODE);
bmp2.set_filter(BMP280_FILTER_COEFF_OFF);
Bmp280Reader bmp_reader(bmp2);
HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 1, 1);
HAL_NVIC_EnableIRQ (DMA1_Stream6_IRQn);
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 1, 0);
HAL_NVIC_EnableIRQ (DMA1_Stream5_IRQn);
#ifndef ENABLE_UART_TASK
uart2.uart_dmarx_start();
#endif
printf("Priority Group: %lu\n", NVIC_GetPriorityGrouping());
printf("SysTick_IRQn priority: %lu\n", NVIC_GetPriority(SysTick_IRQn) << __NVIC_PRIO_BITS);
printf("configKERNEL_INTERRUPT_PRIORITY: %d\n", configKERNEL_INTERRUPT_PRIORITY);
printf("configMAX_SYSCALL_INTERRUPT_PRIORITY: %d\n", configMAX_SYSCALL_INTERRUPT_PRIORITY);
printf("LPS25HB Device id: %d\n", lps25hb.Get_DeviceID());
vTaskDelay(500 / portTICK_RATE_MS);
gyro_reader.init(gyro_wtm);
gyro_reader.enable_int2(false);
vTaskDelay(500 / portTICK_RATE_MS);
acc_reader.init(acc_wtm);
acc_reader.enable_int2(false);
acc_reader.mag_calibrator_.set_bias(drone_state->mag_bias_);
acc_reader.mag_calibrator_.set_scale_factor(drone_state->mag_scale_factor_);
vTaskDelay(500 / portTICK_RATE_MS);
printf("Calibrating...");
gyro_reader.enable_int2(true);
gyro_reader.calculate_static_bias_filtered(2400);
printf(" Done!\n");
flight_ctl.start_receiver();
printf("Entering main loop...\n");
gyro_reader.enable_int2(true);
sample_dt.time_stamp();
lps25hb.Set_FifoMode(LPS25HB_FIFO_STREAM_MODE);
lps25hb.Set_FifoModeUse(LPS25HB_ENABLE);
lps25hb.Set_Odr(LPS25HB_ODR_25HZ);
lps25hb.Set_Bdu(LPS25HB_BDU_NO_UPDATE);
LPS25HB_FIFOTypeDef_st fifo_config;
memset(&fifo_config, 0, sizeof(fifo_config));
lps25hb.Get_FifoConfig(&fifo_config);
#ifdef USE_LPS25HB
float base_pressure = lps25hb.Get_PressureHpa();
for (int i = 0; i < 100; i++) {
while (lps25hb.Get_FifoStatus().FIFO_EMPTY)
vTaskDelay(50 / portTICK_RATE_MS);
base_pressure = pressure_lpf.do_filter(lps25hb.Get_PressureHpa());
}
#endif
bmp_reader.calibrate();
// Infinite loop
PerfCounter idle_time;
while (1) {
drone_state->iteration_++;
if (drone_state->iteration_ % 120 == 0) {
led1.toggle();
}
if (drone_state->iteration_ % 4 == 0) {
#ifdef USE_LPS25HB
drone_state->temperature_ = lps25hb.Get_TemperatureCelsius();
while (!lps25hb.Get_FifoStatus().FIFO_EMPTY) {
drone_state->pressure_hpa_ = pressure_lpf.do_filter(lps25hb.Get_PressureHpa());
float alt = (powf(base_pressure/drone_state->pressure_hpa_, 0.1902f) - 1.0f) * ((lps25hb.Get_TemperatureCelsius()) + 273.15f)/0.0065;
drone_state->altitude_ = Distance::from_meters(alt);
}
#else
bmp_reader.pressure_filter_.set_alpha(drone_state->altitude_lpf_);
drone_state->altitude_ = bmp_reader.get_altitude(true);
drone_state->pressure_hpa_ = bmp_reader.get_pressure().hpa();
drone_state->temperature_ = bmp_reader.get_temperature(false).celsius();
#endif
}
idle_time.begin_measure();
gyro_reader.wait_for_data();
idle_time.end_measure();
drone_state->dt_ = sample_dt.elapsed();
sample_dt.time_stamp();
if (drone_state->base_throttle_ > 0.1)
att.accelerometer_correction_speed(drone_state->accelerometer_correction_speed_);
else
att.accelerometer_correction_speed(3.0f);
att.gyro_drift_pid(drone_state->gyro_drift_kp_, drone_state->gyro_drift_ki_, drone_state->gyro_drift_kd_);
att.gyro_drift_leak_rate(drone_state->gyro_drift_leak_rate_);
size_t fifosize = gyro_reader.size();
for (size_t i = 0; i < fifosize; i++)
drone_state->gyro_raw_ = gyro_lpf.do_filter(gyro_reader.read_sample());
if (drone_state->gyro_raw_.length_squared() > 0 && drone_state->dt_.microseconds() > 0) {
drone_state->gyro_ = (drone_state->gyro_raw_ - gyro_reader.bias()) * drone_state->gyro_factor_;
att.track_gyroscope(DEG2RAD(drone_state->gyro_) * 1.0f, drone_state->dt_.seconds_float());
}
fifosize = acc_reader.size();
for (size_t i = 0; i < fifosize; i++) {
Vector3f acc_sample = acc_reader.read_sample_acc();
acc_lpf_att.do_filter(acc_sample);
acc_lpf_alt.do_filter(acc_sample);
}
drone_state->accel_raw_ = acc_lpf_att.output();
drone_state->accel_alt_ = acc_lpf_alt.output();
drone_state->accel_ = (drone_state->accel_raw_ - drone_state->accelerometer_adjustment_).normalize();
#define ALLOW_ACCELEROMETER_OFF
#ifdef ALLOW_ACCELEROMETER_OFF
if (drone_state->track_accelerometer_) {
att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float());
}
#else
att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float());
#endif
#define REALTIME_DATA 0
#if REALTIME_DATA
std::cout << drone_state->gyro_.transpose() << drone_state->accel_.transpose() << drone_state->pid_torque_.transpose();
std::cout << drone_state->dt_.seconds_float() << std::endl;
#endif
drone_state->mag_raw_ = acc_reader.read_sample_mag();
drone_state->mag_ = drone_state->mag_raw_.normalize();
if (drone_state->track_magnetometer_) {
att.track_magnetometer(drone_state->mag_, drone_state->dt_.seconds_float());
}
drone_state->attitude_ = att.get_attitude();
drone_state->gyro_drift_error_ = RAD2DEG(att.get_drift_error());
flight_ctl.update_state(*drone_state);
flight_ctl.send_throttle_to_motors();
if (print_to_console && console_update_time.elapsed() > TimeSpan::from_milliseconds(300)) {
Vector3f drift_err = att.get_drift_error();
console_update_time.time_stamp();
printf("Gyro : %5.3f %5.3f %5.3f\n", drone_state->gyro_.at(0), drone_state->gyro_.at(1), drone_state->gyro_.at(2));
printf("Drift Err : %5.3f %5.3f %5.3f\n", RAD2DEG(drift_err.at(0)), RAD2DEG(drift_err.at(1)), RAD2DEG(drift_err.at(2)));
printf("Gyro Raw : %5.3f %5.3f %5.3f\n", drone_state->gyro_raw_.at(0), drone_state->gyro_raw_.at(1), drone_state->gyro_raw_.at(2));
printf("Accel : %5.3f %5.3f %5.3f\n", drone_state->accel_.at(0), drone_state->accel_.at(1), drone_state->accel_.at(2));
printf("Mag : %5.3f %5.3f %5.3f\n", drone_state->mag_.at(0), drone_state->mag_.at(1), drone_state->mag_.at(2));
printf("dT : %lu uSec\n", (uint32_t)drone_state->dt_.microseconds());
printf("Q : %5.3f %5.3f %5.3f %5.3f\n\n", drone_state->attitude_.w, drone_state->attitude_.x, drone_state->attitude_.y,
drone_state->attitude_.z);
#if 1
printf("Motors : %1.2f %1.2f %1.2f %1.2f\n", drone_state->motors_[0], drone_state->motors_[1],
drone_state->motors_[2], drone_state->motors_[3]);
printf("Throttle : %1.2f\n", drone_state->base_throttle_);
printf("Armed : %d\n", drone_state->motors_armed_);
printf("Altitude : %4.2f m\n", drone_state->altitude_.meters());
printf("GPS : Lon: %3.4f Lat: %3.4f Sat %lu Alt: %4.2f m\n",
drone_state->longitude_.degrees(), drone_state->latitude_.degrees(),
drone_state->satellite_count_, drone_state->gps_altitude_.meters());
printf("Battery : %2.1f V, %2.0f%%\n", drone_state->battery_voltage_.volts(), drone_state->battery_percentage_);
#endif
}
#if 0
if (led_toggle_ts.elapsed() > TimeSpan::from_seconds(1)) {
led_toggle_ts.time_stamp();
led0.toggle();
}
#endif
#ifndef ENABLE_UART_TASK
rpcserver.jsonrpc_request_handler(&uart2);
#endif
}
}
