void iota_gfx_task_user(void) {
#if DEBUG_TO_SCREEN
if (debug_enable) {
return;
}
#endif
struct CharacterMatrix matrix;
matrix_clear(&matrix);
matrix_write_P(&matrix, PSTR("TKC1800"));
uint8_t layer = biton32(layer_state);
char buf[40];
snprintf(buf,sizeof(buf), "Undef-%d", layer);
matrix_write_P(&matrix, PSTR("\nLayer: "));
matrix_write(&matrix, layer_lookup[layer]);
// Host Keyboard LED Status
char led[40];
snprintf(led, sizeof(led), "\n\n%s %s %s",
(host_keyboard_leds() & (1<<USB_LED_NUM_LOCK)) ? "NUMLOCK" : " ",
(host_keyboard_leds() & (1<<USB_LED_CAPS_LOCK)) ? "CAPS" : " ",
(host_keyboard_leds() & (1<<USB_LED_SCROLL_LOCK)) ? "SCLK" : " ");
matrix_write(&matrix, led);
matrix_update(&display, &matrix);
}
int test_matrix_update(void) {
Matrix *matrix = new_matrix();
int res = fill_matrix(matrix, 20, 20);
if(res != 0) {
LOG_ERR("test matrix update -> fill matrix error");
return res;
}
Element *new_element = element_new(10, 10, 10+10);
Status status = matrix_update(matrix, new_element);
if(status != STAT_SUCCESS) {
LOG_ERR("test matrix update");
return 1;
} else {
Element *element = matrix_find_by_pos(matrix, 10, 10);
if(element == NULL) {
LOG_ERR("test matrix update -> find element error");
return 1;
} else {
if(new_element->value == element->value) {
LOG_SUCCESS("test matrix update");
return 0;
} else {
LOG_ERR("test matrix update, can't update");
return 1;
}
}
}
}
// run a full DCM update round
void
AP_AHRS_DCM::update(void)
{
float delta_t;
// tell the IMU to grab some data
_ins.update();
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time();
// if the update call took more than 0.2 seconds then discard it,
// otherwise we may move too far. This happens when arming motors
// in ArduCopter
if (delta_t > 0.2f) {
_ra_sum.zero();
_ra_deltat = 0;
return;
}
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
}
// run a full DCM update round
void
AP_AHRS_DCM::update(void)
{
float delta_t;
// tell the IMU to grab some data
_imu->update();
// ask the IMU how much time this sensor reading represents
delta_t = _imu->get_delta_time();
// Get current values for gyros
_gyro_vector = _imu->get_gyro();
_accel_vector = _imu->get_accel();
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
}
void
AP_DCM::update_DCM(void)
{
read_adc_raw(); // Get current values for IMU sensors
matrix_update(); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
}
void
AP_DCM_FW::update_DCM(float _G_Dt)
{
_gyro_vector = _imu.get_gyro(); // Get current values for IMU sensors
_accel_vector = _imu.get_accel(); // Get current values for IMU sensors
matrix_update(_G_Dt); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
}
int main(int argc, char **argv)
{
int retval = 0;
if (app_init(argc, argv)) {
retval = -1;
goto out;
}
/*if (matrix_cmd(MATRIX_MODE_GRAYSCALE)) {
retval = -1;
goto out;
}*/
picture_t *pic = picture_alloc();
int i;
int timeval;
const unsigned int start = 100000;
for(i = 0 ; i < 176 ; i++)
{
if(i<60)
timeval = (double)start - (double)(start/1000)*pow(i,1.6);
else
timeval = (double)start - (double)(start/1000)*pow(60,1.6) - (double)(start/1000)*pow(i-60,1.2);
picture_full(pic);
matrix_update(pic);
usleep(timeval);
picture_clear(pic);
matrix_update(pic);
usleep(timeval);
}
picture_free(pic);
out:
app_close();
return retval;
}
// run a full DCM update round
void
AP_AHRS_DCM::update(bool skip_ins_update)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
float delta_t;
if (_last_startup_ms == 0) {
_last_startup_ms = AP_HAL::millis();
load_watchdog_home();
}
if (!skip_ins_update) {
// tell the IMU to grab some data
AP::ins().update();
}
const AP_InertialSensor &_ins = AP::ins();
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time();
// if the update call took more than 0.2 seconds then discard it,
// otherwise we may move too far. This happens when arming motors
// in ArduCopter
if (delta_t > 0.2f) {
memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
return;
}
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Perform drift correction
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
// update trig values including _cos_roll, cos_pitch
update_trig();
// update AOA and SSA
update_AOA_SSA();
backup_attitude();
}
void game_step(void) {
int x = getcurx(main_window);
int y = getcury(main_window);
wmove(panel_window, 0, 0);
matrix_update();
gettimeofday(&now, NULL);
int time = game_delay - (now.tv_usec - before.tv_usec)/1000.0;
if (time > 0) delay_output(time);
gettimeofday(&before, NULL);
wmove(main_window, y, x);
}
void iota_gfx_task_user(void) {
struct CharacterMatrix matrix;
matrix_clear(&matrix);
if (is_master) {
matrix_write(&matrix, read_mode_icon(!get_enable_kc_lang()));
matrix_write(&matrix, " ");
matrix_write(&matrix, read_layer_state());
matrix_write(&matrix, read_host_led_state());
} else {
matrix_write(&matrix, read_logo());
}
matrix_update(&display, &matrix);
}
void iota_gfx_task_user(void) {
struct CharacterMatrix matrix;
#if DEBUG_TO_SCREEN
if (debug_enable) { return; }
#endif
matrix_clear(&matrix);
if (is_master) {
render_status(&matrix);
} else {
render_logo(&matrix);
}
matrix_update(&display, &matrix);
}
void
AP_DCM::update_DCM(void)
{
float delta_t;
_imu->update();
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
_accel_vector = _imu->get_accel(); // Get current values for IMU sensors
delta_t = _imu->get_delta_time();
matrix_update(delta_t); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
}
void
AP_DCM::update_DCM_fast(void)
{
float delta_t;
_imu->update();
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
_accel_vector = _imu->get_accel(); // Get current values for IMU sensors
delta_t = _imu->get_delta_time();
matrix_update(delta_t); // Integrate the DCM matrix
switch(_toggle++){
case 0:
normalize(); // Normalize the DCM matrix
break;
case 1:
//drift_correction(); // Normalize the DCM matrix
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
break;
case 2:
drift_correction(); // Normalize the DCM matrix
break;
case 3:
//drift_correction(); // Normalize the DCM matrix
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
break;
case 4:
euler_yaw();
break;
default:
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
_toggle = 0;
//drift_correction(); // Normalize the DCM matrix
break;
}
}
// run a full DCM update round
void
BC_AHRS::update(void)
{
float delta_t;
// GYRO+ACCの取得
_ins.get_data();
// GYRO+ACCの取得にかかった時間を取得
delta_t = _ins.get_delta_time();
// 取得時間が0.2sec以上だったら捨てる
// CopterにおいてArm時等にそうなる
if (delta_t > 0.2f) {
memset(&_ra_sum, 0, sizeof(_ra_sum)); // _ra_sumを0で埋めてる
_ra_deltat = 0;
return;
}
// Integrate the DCM matrix using gyro inputs
// (超訳)ジャイロ値で方向余弦行列を更新 // ★チェックOK
matrix_update(delta_t);
// Normalize the DCM matrix
// (超訳)方向余弦行列をノーマライズ // ★チェックOK
normalize();
// Perform drift correction
// (超訳)ドリフト補正を実施
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
// (超訳)方向余弦行列中の不正値をチェック
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
// (超訳)ロール、ピッチ、ヨーを計算
euler_angles();
// update trig values including _cos_roll, cos_pitch
// (超訳)必要な値(ex. rollのcos値,等)を計算
update_trig();
}
// run a full DCM update round
void
AP_AHRS_DCM::update(int16_t attitude[3], float delta_t)
{
// Get current values for gyros
_gyro_vector = gyro_attitude;
_accel_vector = accel;
// Integrate the DCM matrix using gyro inputs
matrix_update(delta_t);
// Normalize the DCM matrix
normalize();
// Perform drift correction
//setCurrentProfiledActivity(DRIFT_CORRECTION);
drift_correction(delta_t);
// paranoid check for bad values in the DCM matrix
//setCurrentProfiledActivity(CHECK_MATRIX);
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
//setCurrentProfiledActivity(EULER_ANGLES);
euler_angles();
//setCurrentProfiledActivity(ANGLESOUTPUT);
attitude[0] = roll * INT16DEG_PI_FACTOR;
attitude[1] = pitch* INT16DEG_PI_FACTOR;
attitude[2] = yaw * INT16DEG_PI_FACTOR;
// Just for info:
int16_t sensor = degrees(roll) * 10;
debugOut.analog[0] = sensor;
sensor = degrees(pitch) * 10;
debugOut.analog[1] = sensor;
sensor = degrees(yaw) * 10;
if (sensor < 0)
sensor += 3600;
debugOut.analog[2] = sensor;
}
void iota_gfx_task_user(void) {
#if DEBUG_TO_SCREEN
if (debug_enable) {
return;
}
#endif
struct CharacterMatrix matrix;
matrix_clear(&matrix);
matrix_write_P(&matrix, PSTR("USB: "));
#ifdef PROTOCOL_LUFA
switch (USB_DeviceState) {
case DEVICE_STATE_Unattached:
matrix_write_P(&matrix, PSTR("Unattached"));
break;
case DEVICE_STATE_Suspended:
matrix_write_P(&matrix, PSTR("Suspended"));
break;
case DEVICE_STATE_Configured:
matrix_write_P(&matrix, PSTR("Connected"));
break;
case DEVICE_STATE_Powered:
matrix_write_P(&matrix, PSTR("Powered"));
break;
case DEVICE_STATE_Default:
matrix_write_P(&matrix, PSTR("Default"));
break;
case DEVICE_STATE_Addressed:
matrix_write_P(&matrix, PSTR("Addressed"));
break;
default:
matrix_write_P(&matrix, PSTR("Invalid"));
}
#endif
// Define layers here, Have not worked out how to have text displayed for each layer. Copy down the number you see and add a case for it below
char buf[40];
snprintf(buf,sizeof(buf), "Undef-%ld", layer_state);
matrix_write_P(&matrix, PSTR("\n\nLayer: "));
switch (layer_state) {
case L_BASE:
matrix_write_P(&matrix, PSTR("Default"));
break;
case L_RAISE:
matrix_write_P(&matrix, PSTR("Raise"));
break;
case L_LOWER:
matrix_write_P(&matrix, PSTR("Lower"));
break;
case L_ADJUST:
matrix_write_P(&matrix, PSTR("ADJUST"));
break;
default:
matrix_write(&matrix, buf);
}
// Host Keyboard LED Status
char led[40];
snprintf(led, sizeof(led), "\n%s %s %s",
(host_keyboard_leds() & (1<<USB_LED_NUM_LOCK)) ? "NUMLOCK" : " ",
(host_keyboard_leds() & (1<<USB_LED_CAPS_LOCK)) ? "CAPS" : " ",
(host_keyboard_leds() & (1<<USB_LED_SCROLL_LOCK)) ? "SCLK" : " ");
matrix_write(&matrix, led);
matrix_update(&display, &matrix);
}
void iota_gfx_task_user(void) {
struct CharacterMatrix matrix;
matrix_clear(&matrix);
matrix_render_user(&matrix);
matrix_update(&display, &matrix);
}
