void HttpServerConnection::write(const void* ptr,size_t len) {
if(!len) return;
finishHeader();
if(out_encoding_chunked)
async_printf("\r\n%zx\r\n",len);
async_write_cpy(ptr,len);
}
void HttpServerConnection::writeResponseCode(int code,const char* message) {
if(write_state != LINE)
ThrowInternalError("cannot write response code");
write_state = HEADER;
async_printf("HTTP/%s %d %s\r\nConnection: %s\r\n%s",(version==HTTP_1_1?"1.1":"1.0"),code,message,
keep_alive?"keep-alive":"close",out_encoding_chunked?"Transfer-Encoding: chunked\r\n":"");
}
void HttpServerConnection::writeHeader(const char* header,const char* value) {
if(write_state == LINE)
writeResponseCode(200,"OK");
else if(write_state != HEADER) // could keep a chain to write after the body if chunk encoded
ThrowInternalError("cannot write response code");
async_printf("%s: %s\r\n",header,value);
}
void Sensors_Read(uint8_t SensorType)
{
int g_x, g_y, g_z, a_x, a_y, a_z, m_x, m_y, m_z;
switch (SensorType)
{
case GYRO:
I2C_ReadDeviceRegister(I2C_COM1, ITG3200_ADDRESS, GYRO_XOUT_H_ITG | (1 << 7), MARG_SENSORS[GYRO]->ReadLength, (uint32_t) MARG_SENSORS[GYRO]->SensorRawValue);
//I2C_ReadDeviceRegister_async(I2C_COM1,L3G3200_ADDRESS, OUT_X_L_G | (1 << 7) ,MARG_SENSORS[GYRO]->ReadLength,(uint32_t)MARG_SENSORS[GYRO]->SensorRawValue);
g_x = ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[1]) | (MARG_SENSORS[GYRO]->SensorRawValue[0] << 8))));
g_y = ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[3]) | (MARG_SENSORS[GYRO]->SensorRawValue[2] << 8))));
g_z = ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[5]) | (MARG_SENSORS[GYRO]->SensorRawValue[4] << 8))));
sync_printf("GYRO: %d %d %d\r\n", g_x, g_y, g_z);
break;
/*init ITG3200*/
case ACC:
//I2C_ReadDeviceRegister(I2C_COM1,LSM303_ACC_ADDRESS, OUT_X_H_A | (1 << 7) ,MARG_SENSORS[ACC]->ReadLength,(uint32_t)MARG_SENSORS[ACC]->SensorRawValue);
I2C_ReadDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_DataX_L | (1 << 7), MARG_SENSORS[ACC]->ReadLength, (uint32_t) MARG_SENSORS[ACC]->SensorRawValue); //ADXL345
a_x = ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[0]) | (MARG_SENSORS[ACC]->SensorRawValue[1] << 8))));
a_y = ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[2]) | (MARG_SENSORS[ACC]->SensorRawValue[3] << 8))));
a_z = ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[4]) | (MARG_SENSORS[ACC]->SensorRawValue[5] << 8))));
sync_printf("ACC: %d %d %d\r\n", a_x, a_y, a_z);
break;
case MAG:
//I2C_ReadDeviceRegister(I2C_COM1,LSM303_MAG_ADDRESS, OUT_X_H_M | (1 << 7) ,MARG_SENSORS[MAG]->ReadLength,(uint32_t)MARG_SENSORS[MAG]->SensorRawValue);
I2C_ReadDeviceRegister(I2C_COM1, HMC5883L_ADDRESS, OUT_X_H_M | (1 << 7), MARG_SENSORS[MAG]->ReadLength, (uint32_t) MARG_SENSORS[MAG]->SensorRawValue);
m_x = ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[1]) | (MARG_SENSORS[MAG]->SensorRawValue[0] << 8))));
m_y = ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[3]) | (MARG_SENSORS[MAG]->SensorRawValue[2] << 8))));
m_z = ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[5]) | (MARG_SENSORS[MAG]->SensorRawValue[4] << 8))));
sync_printf("%d %d %d\r\n", m_x, m_y, m_z);
break;
default:
I2C_ReadDeviceRegister(I2C_COM1, ITG3200_ADDRESS, GYRO_XOUT_H_ITG | (1 << 7), MARG_SENSORS[GYRO]->ReadLength, (uint32_t) MARG_SENSORS[GYRO]->SensorRawValue);
//I2C_ReadDeviceRegister_async(I2C_COM1,L3G3200_ADDRESS, OUT_X_L_G | (1 << 7) ,MARG_SENSORS[GYRO]->ReadLength,(uint32_t)MARG_SENSORS[GYRO]->SensorRawValue);
g_x = ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[1]) | (MARG_SENSORS[GYRO]->SensorRawValue[0] << 8))));
g_y = ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[3]) | (MARG_SENSORS[GYRO]->SensorRawValue[2] << 8))));
g_z = ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[5]) | (MARG_SENSORS[GYRO]->SensorRawValue[4] << 8))));
I2C_ReadDeviceRegister(I2C_COM1, ADXL345_ADDRESS, Register_DataX_L | (1 << 7), MARG_SENSORS[ACC]->ReadLength, (uint32_t) MARG_SENSORS[ACC]->SensorRawValue); //ADXL345
a_x = ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[0]) | (MARG_SENSORS[ACC]->SensorRawValue[1] << 8))));
a_y = ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[2]) | (MARG_SENSORS[ACC]->SensorRawValue[3] << 8))));
a_z = ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[4]) | (MARG_SENSORS[ACC]->SensorRawValue[5] << 8))));
I2C_ReadDeviceRegister(I2C_COM1, HMC5883L_ADDRESS, OUT_X_H_M | (1 << 7), MARG_SENSORS[MAG]->ReadLength, (uint32_t) MARG_SENSORS[MAG]->SensorRawValue);
m_x = ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[1]) | (MARG_SENSORS[MAG]->SensorRawValue[0] << 8))));
m_y = ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[3]) | (MARG_SENSORS[MAG]->SensorRawValue[2] << 8))));
m_z = ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[5]) | (MARG_SENSORS[MAG]->SensorRawValue[4] << 8))));
async_printf("%d %d %d %d %d %d %d %d %d\r\n", a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z);
break;
}
}
