void pixelize(Bitmap& bmp, const pixelize_range_t& width){
static_assert(pixelize_range_t::min_allowed > 0,
"pixelize expects min bound > 0");
int w(width.GetValue());
IntSize sz(bmp.GetSize());
Bitmap bmp2(sz);
for (int y = 0; y < sz.h; y +=w){
for (int x = 0; x < sz.w; x +=w){
int r = 0;
int g = 0;
int b = 0;
int a = 0;
int count = 0;
for (int j = 0; y + j != sz.h && j != w; j++){
for (int i = 0; x + i != sz.w && i != w; i++){
Color c = get_color_raw(bmp, x + i, y + j);
r += c.r;
g += c.g;
b += c.b;
a += c.a;
count++;
}
}
Color c2(color_from_ints(r / count, g / count, b / count, a / count));
for (int j = 0; y + j < sz.h && j != w; j++){
for (int i = 0; x + i != sz.w && i != w; i++){
put_pixel_raw(bmp2, x + i, y + j, c2);
}
}
}
}
bmp = bmp2;
}
wxIcon &GetGreyIcon(wxIcon &icon)
{
wxBitmap bmp;
bmp.CopyFromIcon(icon);
wxImage img = bmp.ConvertToImage();
GreyOutImage(img);
wxBitmap bmp2(img,32);
static wxIcon rval;
rval.CopyFromBitmap(bmp2);
return rval;
}
// 绘制羽化文字
void DrawEclosionText(HDC hDC, LPCTSTR lpszText, LPCTSTR lpszFontName, int nFontSize)
{
int nWidth = 56, nHeight = 16;
Gdiplus::Bitmap bmp(nWidth, nHeight, 0, PixelFormat32bppARGB, NULL);
Gdiplus::Graphics graphics(&bmp);
Gdiplus::GraphicsPath path(Gdiplus::FillModeAlternate);
Gdiplus::FontFamily fontFamily(lpszFontName, NULL);
Gdiplus::StringFormat strFmt(0, 0);
Gdiplus::RectF rc(0.0f, 0.0f, nWidth, nHeight);
path.AddString(lpszText, -1, &fontFamily, Gdiplus::FontStyleBold, nFontSize, rc, &strFmt);
Gdiplus::Matrix matrix(1.0f/* / 5.0f*/, 0.0f, 0.0f, 1.0f/* / 5.0f*/, -1.0f/* / 5.0f*/, -1.0f/* / 5.0f*/);
graphics.SetSmoothingMode(Gdiplus::SmoothingModeAntiAlias);
graphics.SetTransform(&matrix);
Gdiplus::Color color((Gdiplus::ARGB)0x55CCE0EE);
Gdiplus::Pen pen(color, 3);
graphics.DrawPath(&pen, &path);
Gdiplus::Color color2((Gdiplus::ARGB)0x55CCE0EE);
Gdiplus::SolidBrush brush(color2);
graphics.FillPath(&brush, &path);
Gdiplus::Bitmap bmp2(300, 40, 0, PixelFormat32bppARGB, NULL);
Gdiplus::Graphics graphics2(&bmp2);
graphics2.SetSmoothingMode(Gdiplus::SmoothingModeAntiAlias);
graphics2.SetInterpolationMode(Gdiplus::InterpolationModeHighQualityBicubic);
graphics2.DrawImage(&bmp, 0, 0, nWidth, nHeight);
Gdiplus::Color color3((Gdiplus::ARGB)0xFF000000);
Gdiplus::SolidBrush brush2(color3);
graphics2.FillPath(&brush2, &path);
CLSID clsid;
::CLSIDFromString(_T("{557CF406-1A04-11D3-9A73-0000F81EF32E}"), &clsid);
bmp2.Save(_T("c:\\CoolText.png"), &clsid, NULL);
Gdiplus::Graphics graphics3(hDC);
graphics3.DrawImage(&bmp2, 8, 4);
}
int WSAPI bitmap_command(ws_handle handle,int command,int param1,int param2)
{
WBitmap &bmp=*(WBitmap *)handle;
switch (command) {
case C_SETPOS:
bmp.SetPos(param1,param2);
break;
case C_CLEAR:
bmp.Clear((DWORD)param1);
break;
case C_BLIT:
if (param2&BLIT_TRANS) {
int alpha=param2>>16;
WBitmap &bmp2=*(WBitmap *)param1;
bmp.Draw(bmp2(alpha),param2&0xffff);
}
else bmp.Draw(*(const WBitmap *)param1,param2);
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
}
}
