Math::vec4 FreeImage::getPixelf( Size x, Size y ) const {
unsigned int iIndex = ( y * this -> size.x + x ) * ( this -> BPP / 8 );
unsigned char b = 0;
unsigned char g = 0;
unsigned char r = 0;
unsigned char a = 0;
switch ( this -> loadingFormat ) {
case Format::R:
r = getDatas()[iIndex];
break;
case Format::RGB:
b = getDatas()[iIndex];
g = getDatas()[iIndex + 1];
r = getDatas()[iIndex + 2];
break;
case Format::RGBA:
b = getDatas()[iIndex];
g = getDatas()[iIndex + 1];
r = getDatas()[iIndex + 2];
a = getDatas()[iIndex + 3];
break;
}
return Math::vec4( float( r ) / 255.0f, float( g ) / 255.0f, float( b ) / 255.0f, float( a ) / 255.0f );
}
uint32_t sendDataPHYSENS(ble_pss_t * p_pss)
{
uint32_t err_code = NRF_SUCCESS;
uint16_t len = (SENSOR_ROW_SIZE);
uint8_t data[SENSOR_ROW_SIZE] = {0};
uint8_t memory[6] = {0}; // 3 datas of 6 bytes each
uint8_t iter, iter_data;
uint16_t new_remember = 0;
uint16_t add = 0;
uint16_t skip = g_index_skip;
if ((p_pss->conn_handle != BLE_CONN_HANDLE_INVALID) && p_pss->is_notification_supported)
{
ble_gatts_hvx_params_t hvx_params;
memset(&hvx_params, 0, sizeof(hvx_params));
hvx_params.handle = p_pss->phy_sen_level_handles.value_handle;
hvx_params.type = BLE_GATT_HVX_NOTIFICATION;
hvx_params.p_len = &len;
switch (ble_mode) {
case BLE_STICK_MODE:
if (g_state == 2) {
data[0] = STICK_MOMENT;
data[1] = g_battery_int;
data[2] = 1;
for (iter=0; iter<10; iter++) {
data[3+iter] = g_data_send[iter];
}
g_state = 3;
} else if (g_state == 3) {
data[0] = STICK_MOMENT;
data[1] = g_battery_int;
data[2] = 2;
for (iter=10; iter<22; iter++) {
data[3+iter-10] = g_data_send[iter];
}
g_state = 0; //RESTART
} else {
//TODO, put code from settings
//Send settings
/*data[0] = STICK_MOMENT;
data[1] = g_battery_int;
for (iter=0; iter<14; iter++) {
data[2+iter] = g_data_send[iter];
}*/
data[0] = SETTINGS_READ;
data[1] = g_battery_int;
for (iter=0; iter<18; iter++) {
data[2+iter] = g_settings[iter];
}
}
break;
case BLE_SETTINGS_MODE:
data[0] = SETTINGS_READ;
data[1] = g_battery_int;
for (iter=0; iter<18; iter++) {
data[2+iter] = g_settings[iter];
}
break;
case BLE_CALIB_AXIS_MODE:
data[0] = CALIB_OUTPUT;
data[1] = g_battery_int;
for (iter=0; iter<8; iter++) {
data[2+iter] = g_calib_axis[iter];
}
for (iter=8; iter<18; iter++) {
data[2+iter] = 0;
}
break;
case BLE_FREE_MODE:
case BLE_OTHER_MODE:
case BLE_SHOT_MODE:
// TODO mode magement
// Draft mode and real mode mangement
// Indexing later
switch(g_state) {
case 0:
case 1:
data[0] = DATA_DRAFT;
data[1] = g_battery_int;
for (iter_data = 0; iter_data<3; iter_data++) {
for (iter=0; iter<6; iter++) {
data[2+iter + (iter_data*6)] = g_data_send[iter + (iter_data*6)];
}
}
break;
case 2:
data[0] = DATA_START;
data[1] = g_battery_int;
// 300 for 30, but 32 indexes system.
// Take ratio (300 / (30/32))
//TODO
if (g_remember < 320) {
g_remember = BR25S_CIRCULAR_BUFFER - (320-g_remember);
} else {
g_remember -= 320;
}
g_shot_br25s_index = 0;
break;
case 3:
data[0] = DATA;
data[1] = g_battery_int;
break;
case 4:
data[0] = DATA_END;
data[1] = g_battery_int;
break;
}
if (g_state >= 2) {
new_remember = g_remember;
for (iter_data=0; iter_data<3; iter_data++) {
getDatas(memory, 6, new_remember);
new_remember += g_skip[g_index_skip % 5];
add += g_skip[g_index_skip % 5];
g_index_skip++;
if (new_remember >= BR25S_CIRCULAR_BUFFER) {
new_remember = 0;
}
for (iter=0; iter<6; iter++) {
data[2+iter + (iter_data*6)] = memory[iter];
}
}
}
break;
}
hvx_params.p_data = data;
err_code = sd_ble_gatts_hvx(p_pss->conn_handle, &hvx_params);
if (ble_mode == BLE_STICK_MODE && g_state <= 1) {
return NRF_ERROR_INVALID_STATE; // Send only one data
} else if (ble_mode == BLE_STICK_MODE) {
return err_code;
}
// Add only if complete data
if (ble_mode == BLE_SHOT_MODE && err_code == NRF_SUCCESS && g_state >= 2) {
if (g_state == 2) {
g_state = 3;
} else if (g_state == 4) {
g_state = 0;
g_real_index = 0; //Flush everything
g_valid = 1;
g_index_skip = 0;
}
g_remember = new_remember;
g_shot_br25s_index += add;
if (g_remember >= BR25S_CIRCULAR_BUFFER)
g_remember == 0;
if (g_state == 3) {
// 18 left + 2 extra(Nothing)
if (g_shot_br25s_index >= BR25S_CIRCULAR_BUFFER - 20) {
// This sample and the last one
g_state = 4;
}
}
} else if (g_state >= 2) {
// Reinit some value
g_index_skip = skip;
}
}
else
{
err_code = NRF_ERROR_INVALID_STATE;
}
if ((ble_mode == BLE_SHOT_MODE || ble_mode == BLE_FREE_MODE) && g_state <= 1) {
return NRF_ERROR_INVALID_STATE; // Send only one data
}
return err_code;
}