void __led_set(led_id_t mask, int state)
{
switch(mask) {
case STATUS_LED_RED:
if(STATUS_LED_ON == state) {
red_led_on();
} else {
red_led_off();
}
break;
case STATUS_LED_GREEN:
if(STATUS_LED_ON == state) {
green_led_on();
} else {
green_led_off();
}
break;
case STATUS_LED_YELLOW:
if(STATUS_LED_ON == state) {
yellow_led_on();
} else {
yellow_led_off();
}
break;
case STATUS_LED_BLUE:
if(STATUS_LED_ON == state) {
blue_led_on();
} else {
blue_led_off();
}
break;
}
}
/**
****************************************************************************************
* @brief Stops the battery low level warning indication on the Red LED
*
* @param None
*
* @return void
****************************************************************************************
*/
void leds_clear_battery_low_warning()
{
// if high priority indication is active
// the LED status is left unchanged
// the LED is left to be handled by the
// high priority indications handler
if (!high_priority_indications_active) {
red_led_off();
}
}
/**
****************************************************************************************
* @brief Sets-up the LEDS to indicate connection astablished status
*
* @param None
*
* @return void
****************************************************************************************
*/
void leds_set_connection_established()
{
if (!(GetWord16(SYS_STAT_REG) & DBG_IS_UP)) {
red_led_off();
green_led_on();
app_timer_set(APP_GREEN_LED_TIMER, TASK_APP, GREEN_ON);
if (app_get_sleep_mode()) {
app_force_active_mode(); // prevent sleep only if enabled
}
}
}
static void memtest(void) {
red_led_off();
while (true) {
memtest_struct.rand_seed = chSysGetRealtimeCounterX();
memtest_run(&memtest_struct, MEMTEST_RUN_ALL);
green_led_toggle();
}
green_led_on();
green_led_off();
}
static THD_FUNCTION(BootBlinkThread, arg) {
chRegSetThreadName("BootBlink");
(void)arg;
while (!chThdShouldTerminateX()) {
red_led_on();
warning_led_off();
PAUSE();
red_led_off();
warning_led_on();
PAUSE();
warning_led_off();
red_led_off();
PAUSE();
}
warning_led_off();
red_led_off();
chThdExit(MSG_OK);
}
void __led_toggle(led_id_t mask)
{
if (STATUS_LED_RED == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_RED])
red_led_off();
else
red_led_on();
} else if (STATUS_LED_GREEN == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_GREEN])
green_led_off();
else
green_led_on();
}
}
void __led_set(led_id_t mask, int state)
{
switch(mask) {
case STATUS_LED_RED:
if(STATUS_LED_ON == state) {
red_led_on();
} else {
red_led_off();
}
break;
case STATUS_LED_GREEN:
if(STATUS_LED_ON == state) {
green_led_on();
} else {
green_led_off();
}
break;
}
}
void __led_set(led_id_t mask, int state)
{
if (STATUS_LED_RED == mask) {
if (STATUS_LED_ON == state)
red_led_on();
else
red_led_off();
} else if (STATUS_LED_GREEN == mask) {
if (STATUS_LED_ON == state)
green_led_on();
else
green_led_off();
} else if (STATUS_LED_YELLOW == mask) {
if (STATUS_LED_ON == state)
yellow_led_on();
else
yellow_led_off();
}
}
void __led_toggle(led_id_t mask)
{
if (STATUS_LED_RED == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_RED])
red_led_off();
else
red_led_on();
} else if (STATUS_LED_GREEN == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_GREEN])
green_led_off();
else
green_led_on();
} else if (STATUS_LED_YELLOW == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_YELLOW])
yellow_led_off();
else
yellow_led_on();
} else if (STATUS_LED_BLUE == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_BLUE])
blue_led_off();
else
blue_led_on();
}
}
static void general_test (NANDDriver *nandp, size_t first,
size_t last, size_t read_rounds){
size_t block, page, round;
bool status;
uint8_t op_status;
uint32_t recc, wecc;
red_led_on();
/* initialize time measurement units */
chTMObjectInit(&tmu_erase);
chTMObjectInit(&tmu_write_data);
chTMObjectInit(&tmu_write_spare);
chTMObjectInit(&tmu_read_data);
chTMObjectInit(&tmu_read_spare);
/* perform basic checks */
for (block=first; block<last; block++){
if (!nandIsBad(nandp, block)){
if (!is_erased(nandp, block)){
op_status = nandErase(nandp, block);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
}
}
}
/* write block with pattern, read it back and compare */
for (block=first; block<last; block++){
if (!nandIsBad(nandp, block)){
for (page=0; page<nandp->config->pages_per_block; page++){
pattern_fill();
chTMStartMeasurementX(&tmu_write_data);
op_status = nandWritePageData(nandp, block, page,
nand_buf, nandp->config->page_data_size, &wecc);
chTMStopMeasurementX(&tmu_write_data);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
chTMStartMeasurementX(&tmu_write_spare);
op_status = nandWritePageSpare(nandp, block, page,
nand_buf + nandp->config->page_data_size,
nandp->config->page_spare_size);
chTMStopMeasurementX(&tmu_write_spare);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
/* read back and compare */
for (round=0; round<read_rounds; round++){
memset(nand_buf, 0, NAND_PAGE_SIZE);
chTMStartMeasurementX(&tmu_read_data);
nandReadPageData(nandp, block, page,
nand_buf, nandp->config->page_data_size, &recc);
chTMStopMeasurementX(&tmu_read_data);
osalDbgCheck(0 == (recc ^ wecc)); /* ECC error detected */
chTMStartMeasurementX(&tmu_read_spare);
nandReadPageSpare(nandp, block, page,
nand_buf + nandp->config->page_data_size,
nandp->config->page_spare_size);
chTMStopMeasurementX(&tmu_read_spare);
osalDbgCheck(0 == memcmp(ref_buf, nand_buf, NAND_PAGE_SIZE)); /* Read back failed */
}
}
/* make clean */
chTMStartMeasurementX(&tmu_erase);
op_status = nandErase(nandp, block);
chTMStopMeasurementX(&tmu_erase);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
status = is_erased(nandp, block);
osalDbgCheck(true == status); /* blocks was not erased successfully */
}/* if (!nandIsBad(nandp, block)){ */
}
red_led_off();
}
int main(){
// Initialize Peripherals
interface_init();
red_led_on();
uart_init(BAUDRATE);
animation_manager_init();
sys_timer_start();
audio_init();
sei(); // enable global interrupts
// Load Default Animation
animation_manager_load_animation(START_ANIMATION);
// Enter Setup if Requested
_delay_ms(100);
if(deb_switch_1()){
setup_wb_run();
}
else if(deb_switch_2()){
setup_orientation_run();
}
// Load Default Animation
animation_manager_load_animation(START_ANIMATION);
// Set White Balance
_delay_ms(300);
display_wb_update();
while(uart_async_run()); // setup white balance
// Control Panel is Ready => Signal this by Turning the LED Green
red_led_off();
green_led_on();
while(1){
// Sleep Mode
if(!switch_on_off()){ // if switched off
go_to_sleep();
}
// Change animations
sw_check();
if(sw_check_pressed(SW_LEFT, 200, true)){
animation_manager_dec_animation();
}
else if(sw_check_pressed(SW_RIGHT, 200, true)){
animation_manager_inc_animation();
}
else if(sw_check_pressed(SW_RAND, 300, true)){
animation_manager_random_animation();
}
// Generate Image
animation_manager_run(0);
// Check Audio
audio_start();
while(audio_run());
audio_process();
// Display Image
while(uart_async_run()){
interface_async_run();
}
}
}
void Navi6dWrapper::update(const baro_data_t &baro,
const odometer_data_t &odo,
const marg_data_t &marg) {
osalDbgCheck(ready);
start_time_measurement();
/* restart sins if requested */
if (*restart != restart_cache) {
sins_cold_start();
restart_cache = *restart;
}
nav_sins.init_params.dT = marg.dT;
nav_sins.kalman_params.gnss_mean_pos_sigma = *R_pos_sns;
nav_sins.kalman_params.gnss_mean_alt_sigma = *R_alt_sns;
nav_sins.kalman_params.gnss_mean_vel_sigma = *R_vel_sns;
nav_sins.kalman_params.sigma_R.v_odo_x = *R_odo; //odo
nav_sins.kalman_params.sigma_R.v_nhl_y = *R_nhl_y; //nonhol
nav_sins.kalman_params.sigma_R.v_nhl_z = *R_nhl_z; //nonhol
nav_sins.kalman_params.sigma_R.alt_baro = *R_baro; //baro
nav_sins.kalman_params.sigma_R.mag_x = *R_mag; //mag
nav_sins.kalman_params.sigma_R.mag_y = *R_mag; //mag
nav_sins.kalman_params.sigma_R.mag_z = *R_mag; //mag
nav_sins.kalman_params.sigma_R.roll = *R_euler; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.pitch = *R_euler; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.yaw = *R_euler; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.v_nav_static = *R_v_nav_st; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.v_veh_static = *R_v_veh_st; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.yaw_static = *R_yaw_st; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.yaw_mag = *R_mag_yaw; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_Qm.acc_x = *Qm_acc; //acc
nav_sins.kalman_params.sigma_Qm.acc_y = *Qm_acc; //acc
nav_sins.kalman_params.sigma_Qm.acc_z = *Qm_acc; //accbias
nav_sins.kalman_params.sigma_Qm.gyr_x = *Qm_gyr; //gyr
nav_sins.kalman_params.sigma_Qm.gyr_y = *Qm_gyr; //gyr
nav_sins.kalman_params.sigma_Qm.gyr_z = *Qm_gyr; //gyr
nav_sins.kalman_params.sigma_Qm.acc_b_x = *Qm_acc_bias; //acc_x
nav_sins.kalman_params.sigma_Qm.acc_b_y = *Qm_acc_bias; //acc_y
nav_sins.kalman_params.sigma_Qm.acc_b_z = *Qm_acc_bias; //acc_z
nav_sins.kalman_params.sigma_Qm.gyr_b_x = *Qm_gyr_bias; //gyr_bias
nav_sins.kalman_params.sigma_Qm.gyr_b_y = *Qm_gyr_bias; //gyr_bias
nav_sins.kalman_params.sigma_Qm.gyr_b_z = *Qm_gyr_bias; //gyr_bias
nav_sins.kalman_params.Beta_inv.acc_b = *B_acc_b;
nav_sins.kalman_params.Beta_inv.gyr_b = *B_gyr_b;
nav_sins.kalman_params.Beta_inv.acc_s = 10000000.0;
nav_sins.kalman_params.Beta_inv.gyr_s = 10000000.0;
nav_sins.kalman_params.Beta_inv.acc_no = 10000000.0;
nav_sins.kalman_params.Beta_inv.gyr_no = 10000000.0;
nav_sins.calib_params.ba[0][0] = *acc_bias_x;
nav_sins.calib_params.ba[1][0] = *acc_bias_y;
nav_sins.calib_params.ba[2][0] = *acc_bias_z;
nav_sins.calib_params.bw[0][0] = *gyr_bias_x;
nav_sins.calib_params.bw[1][0] = *gyr_bias_y;
nav_sins.calib_params.bw[2][0] = *gyr_bias_z;
nav_sins.calib_params.sa[0][0] = *acc_scale_x;
nav_sins.calib_params.sa[1][0] = *acc_scale_y;
nav_sins.calib_params.sa[2][0] = *acc_scale_z;
nav_sins.calib_params.sw[0][0] = *gyr_scale_x;
nav_sins.calib_params.sw[1][0] = *gyr_scale_y;
nav_sins.calib_params.sw[2][0] = *gyr_scale_z;
nav_sins.ref_params.mag_dec = deg2rad(*mag_declinate);
nav_sins.calib_params.bm_marg[0][0] = *mag_bias_x;
nav_sins.calib_params.bm_marg[1][0] = *mag_bias_y;
nav_sins.calib_params.bm_marg[2][0] = *mag_bias_z;
nav_sins.calib_params.sm_marg[0][0] = *mag_scale_x;
nav_sins.calib_params.sm_marg[1][0] = *mag_scale_y;
nav_sins.calib_params.sm_marg[2][0] = *mag_scale_z;
nav_sins.calib_params.no_m_marg[0][0] = *mag_nort_x;
nav_sins.calib_params.no_m_marg[1][0] = *mag_nort_y;
nav_sins.calib_params.no_m_marg[2][0] = *mag_nort_z;
/*
nav_sins.calib_params.no_a[0][0] = *acc_nort_0;
nav_sins.calib_params.no_a[1][0] = *acc_nort_1;
nav_sins.calib_params.no_a[2][0] = *acc_nort_2;
nav_sins.calib_params.no_a[3][0] = *acc_nort_3;
nav_sins.calib_params.no_a[4][0] = *acc_nort_4;
nav_sins.calib_params.no_a[5][0] = *acc_nort_5;
nav_sins.calib_params.no_w[0][0] = *gyr_nort_0;
nav_sins.calib_params.no_w[1][0] = *gyr_nort_1;
nav_sins.calib_params.no_w[2][0] = *gyr_nort_2;
nav_sins.calib_params.no_w[3][0] = *gyr_nort_3;
nav_sins.calib_params.no_w[4][0] = *gyr_nort_4;
nav_sins.calib_params.no_w[5][0] = *gyr_nort_5;
*/
nav_sins.calib_params.ba[0][0] = *acc_bias_x;
nav_sins.calib_params.ba[1][0] = *acc_bias_y;
nav_sins.calib_params.ba[2][0] = *acc_bias_z;
nav_sins.calib_params.bw[0][0] = *gyr_bias_x;
nav_sins.calib_params.bw[1][0] = *gyr_bias_y;
nav_sins.calib_params.bw[2][0] = *gyr_bias_z;
nav_sins.calib_params.sa[0][0] = *acc_scale_x;
nav_sins.calib_params.sa[1][0] = *acc_scale_y;
nav_sins.calib_params.sa[2][0] = *acc_scale_z;
nav_sins.calib_params.sw[0][0] = *gyr_scale_x;
nav_sins.calib_params.sw[1][0] = *gyr_scale_y;
nav_sins.calib_params.sw[2][0] = *gyr_scale_z;
nav_sins.marg_b.eu_bs_mag[0][0] = M_PI;
nav_sins.marg_b.eu_bs_mag[1][0] = 0.0;
nav_sins.marg_b.eu_bs_mag[2][0] = -M_PI;
nav_sins.ref_params.eu_vh_base[0][0] = *eu_vh_roll;
nav_sins.ref_params.eu_vh_base[1][0] = *eu_vh_pitch;
nav_sins.ref_params.eu_vh_base[2][0] = *eu_vh_yaw;
nav_sins.ref_params.eu_vh_base[0][0] = *eu_vh_roll;
nav_sins.ref_params.eu_vh_base[1][0] = *eu_vh_pitch;
nav_sins.ref_params.eu_vh_base[2][0] = *eu_vh_yaw;
nav_sins.ref_params.zupt_source = *zupt_src;
nav_sins.ref_params.glrt_gamma = *gamma;
nav_sins.ref_params.glrt_acc_sigma = *acc_sigma;
nav_sins.ref_params.glrt_gyr_sigma = *gyr_sigma;
nav_sins.ref_params.glrt_n = *samples;
nav_sins.ref_params.sns_extr_en = *en_extr;
nav_sins.ref_params.sns_vel_th = *sns_v_th;
dbg_in_fill_gnss(this->gps);
prepare_data_gnss(this->gps);
dbg_in_fill_other(baro, odo, marg);
dbg_in_append_log();
prepare_data(baro, odo, marg);
nav_sins.run();
#if defined(BOARD_BEZVODIATEL)
if (NAV_RUN_STATIONARY_AUTONOMOUS == nav_sins.run_mode ||
NAV_RUN_STATIONARY_PRIMARY == nav_sins.run_mode) {
blue_led_on();
red_led_on();
} else {
blue_led_off();
red_led_off();
}
#endif
navi2acs();
navi2mavlink();
debug2mavlink();
dbg_out_fill(nav_sins.navi_data);
dbg_out_append_log();
stop_time_measurement(marg.dT);
}
bool process_record_user(uint16_t keycode, keyrecord_t *record) {
switch (keycode) {
case QWERTY:
if (record->event.pressed) {
set_single_persistent_default_layer(_QWERTY);
}
return false;
break;
case LOWER:
if (record->event.pressed) {
layer_on(_LOWER);
grn_led_on();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_LOWER);
grn_led_off();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
//SHIFT is handled as LSHIFT in the general case - 'toggle' shoudl activate caps, while the layer is only active when shift is held.
case RAISE:
if (record->event.pressed) {
layer_on(_RAISE);
red_led_on();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_RAISE);
red_led_off();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
case ADJUST:
if (record->event.pressed) {
layer_on(_ADJUST);
} else {
layer_off(_ADJUST);
}
return false;
break;
//MOUSE layer needs to be handled the same way as NUMKEY, but differently from shift
case MOUKEY:
if (record->event.pressed) {
layer_on(_MOUSE);
blu_led_on();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_MOUSE);
blu_led_off();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
//mouse buttons, for 1-3, to update the mouse report:
case MS_BTN1:
currentReport = pointing_device_get_report();
if (record->event.pressed) {
if (shift_held && shift_suspended){
register_code(KC_LSFT);
shift_suspended = false;
}
//update mouse report here
currentReport.buttons |= MOUSE_BTN1; //MOUSE_BTN1 is a const defined in report.h
} else {
//update mouse report here
currentReport.buttons &= ~MOUSE_BTN1;
}
pointing_device_set_report(currentReport);
return false;
break;
case MS_BTN2:
currentReport = pointing_device_get_report();
if (record->event.pressed) {
if (shift_held && shift_suspended){
register_code(KC_LSFT);
shift_suspended = false;
}
//update mouse report here
currentReport.buttons |= MOUSE_BTN2; //MOUSE_BTN2 is a const defined in report.h
} else {
//update mouse report here
currentReport.buttons &= ~MOUSE_BTN2;
}
pointing_device_set_report(currentReport);
return false;
break;
case MS_BTN3:
currentReport = pointing_device_get_report();
if (record->event.pressed) {
if (shift_held && shift_suspended){
register_code(KC_LSFT);
shift_suspended = false;
}
//update mouse report here
currentReport.buttons |= MOUSE_BTN3; //MOUSE_BTN3 is a const defined in report.h
} else {
//update mouse report here
currentReport.buttons &= ~MOUSE_BTN3;
}
pointing_device_set_report(currentReport);
return false;
break;
//Additionally, if NS_ keys are in use, then shift may be held (but is
//disabled for the unshifted keycodes to be send. Check the bool and
//register shift as necessary.
// default:
// if (shift_held){
// register_code(KC_LSFT);
// }
// break;
}
return true;
};
/**
****************************************************************************************
* @brief Handler of the Red LED Timer
*
* @param[in] msgid
* @param[in] param
* @param[in] dest_id
* @param[in] src_id
*
* @return KE_MSG_CONSUMED
****************************************************************************************
*/
int app_red_led_timer_handler(ke_msg_id_t const msgid,
void const *param,
ke_task_id_t const dest_id,
ke_task_id_t const src_id)
{
if (!dbg_uses_led_pins() || !((GetWord16(SYS_STAT_REG) & DBG_IS_UP) == DBG_IS_UP)) {
// GPIOs are not being used by the debugger
switch(red_led_st) {
case LED_OFF:
red_led_off();
break;
case DOUBLE_BLINK_LED_IS_ON__TURN_OFF_A:
red_led_off();
app_timer_set(APP_RED_LED_TIMER, TASK_APP, DOUBLE_BLINK_RED_OFF_A);
red_led_st = DOUBLE_BLINK_LED_IS_OFF__TURN_ON_B;
if ((GetBits16(ANA_STATUS_REG, BOOST_SELECTED) == 0x1) &&
((green_led_st == LED_OFF) || (green_led_st == BLINK_LED_IS_OFF__TURN_ON))) {
app_restore_sleep_mode();
}
break;
case DOUBLE_BLINK_LED_IS_OFF__TURN_ON_B:
red_led_blink();
app_timer_set(APP_RED_LED_TIMER, TASK_APP, DOUBLE_BLINK_RED_ON_B);
red_led_st = DOUBLE_BLINK_LED_IS_ON__TURN_OFF_B;
leds_block_sleep();
break;
case DOUBLE_BLINK_LED_IS_ON__TURN_OFF_B:
red_led_off();
app_timer_set(APP_RED_LED_TIMER, TASK_APP, DOUBLE_BLINK_RED_OFF_B);
red_led_st = LED_OFF;
if ((GetBits16(ANA_STATUS_REG, BOOST_SELECTED) == 0x1) &&
((green_led_st == LED_OFF) || (green_led_st == BLINK_LED_IS_OFF__TURN_ON))) {
app_restore_sleep_mode();
}
break;
case BLINK_LED_IS_ON__TURN_OFF:
red_led_off();
app_timer_set(APP_RED_LED_TIMER, TASK_APP, BLINK_RED_OFF);
red_led_st = BLINK_LED_IS_OFF__TURN_ON;
if ((GetBits16(ANA_STATUS_REG, BOOST_SELECTED) == 0x1) &&
((green_led_st == LED_OFF) || (green_led_st == BLINK_LED_IS_OFF__TURN_ON))){
app_restore_sleep_mode(); // restore sleep
}
break;
case BLINK_LED_IS_OFF__TURN_ON:
red_led_blink();
app_timer_set(APP_RED_LED_TIMER, TASK_APP, BLINK_RED_ON);
leds_block_sleep();
break;
case LED_ON:
red_led_off();
if ((GetBits16(ANA_STATUS_REG, BOOST_SELECTED) == 0x1) &&
((green_led_st == LED_OFF) || (green_led_st == BLINK_LED_IS_OFF__TURN_ON))) {
app_restore_sleep_mode(); // restore sleep
}
high_priority_indications_active = false;
break;
default:
break;
}
}
return (KE_MSG_CONSUMED);
}