upm_result_t bno055_set_operation_mode(const bno055_context dev,
BNO055_OPERATION_MODES_T mode)
{
assert(dev != NULL);
if (bno055_set_page(dev, 0, false))
return UPM_ERROR_OPERATION_FAILED;
// we clear all of our loaded data on mode changes
_clear_data(dev);
uint8_t reg = 0;
if (bno055_read_reg(dev, BNO055_REG_OPER_MODE, ®))
return UPM_ERROR_OPERATION_FAILED;
reg &= ~(_BNO055_OPR_MODE_OPERATION_MODE_MASK
<< _BNO055_OPR_MODE_OPERATION_MODE_SHIFT);
reg |= (mode << _BNO055_OPR_MODE_OPERATION_MODE_SHIFT);
if (bno055_write_reg(dev, BNO055_REG_OPER_MODE, reg))
return UPM_ERROR_OPERATION_FAILED;
dev->currentMode = mode;
upm_delay_us(30);
return UPM_SUCCESS;
}
// init
bno055_context bno055_init(int bus, uint8_t addr)
{
bno055_context dev =
(bno055_context)malloc(sizeof(struct _bno055_context));
if (!dev)
return NULL;
// zero out context
memset((void *)dev, 0, sizeof(struct _bno055_context));
// make sure MRAA is initialized
int mraa_rv;
if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
{
printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
bno055_close(dev);
return NULL;
}
if (!(dev->i2c = mraa_i2c_init(bus)))
{
printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
if (mraa_i2c_address(dev->i2c, addr) != MRAA_SUCCESS)
{
printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
_clear_data(dev);
// forcibly set page 0, so we are synced with the device
if (bno055_set_page(dev, 0, true))
{
printf("%s: bno055_set_page() failed.\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
// check the chip id. This has to be done after forcibly setting
// page 0, as that is the only page where the chip id is present.
uint8_t chipID = 0;
if (bno055_get_chip_id(dev, &chipID))
{
printf("%s: Could not read chip id\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
if (chipID != BNO055_CHIPID)
{
printf("%s: Invalid chip ID. Expected 0x%02x, got 0x%02x\n",
__FUNCTION__, BNO055_CHIPID, chipID);
bno055_close(dev);
return NULL;
}
upm_result_t urv = UPM_SUCCESS;
// set config mode
urv += bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE);
// default to internal clock
urv += bno055_set_clock_external(dev, false);
// we specifically avoid doing a reset so that if the device is
// already calibrated, it will remain so.
// we always use C for temperature
urv += bno055_set_temperature_units_celsius(dev);
// default to accelerometer temp
urv += bno055_set_temperature_source(dev, BNO055_TEMP_SOURCE_ACC);
// set accel units to m/s^2
urv += bno055_set_accelerometer_units(dev, false);
// set gyro units to degrees
urv += bno055_set_gyroscope_units(dev, false);
// set Euler units to degrees
urv += bno055_set_euler_units(dev, false);
// by default, we set the operating mode to the NDOF fusion mode
urv += bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_NDOF);
// if any of those failed, bail
if (urv != UPM_SUCCESS)
{
printf("%s: Initial device configuration failed\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
return dev;
}
void HttpHandler::start_handle()
{
printf("start handler http\n");
sock_file_ = fdopen(http_fd_,"a+");
if (NULL == sock_file_)
_clear_data();
//read first line of HTTP METHOD
_read_request();
//read option
for (int i = 0; i < MAX_HTTP_FIELD_PAIR_NUM; i++) {
if (http_arg_info_[i].key)
printf("%s\t:\t%s\n",http_arg_info_[i].key,http_arg_info_[i].value);
}
//read till \r\n,empty line
while (fgets(http_option_data_,MAX_HTTP_LINE_SIZE,sock_file_) && strcmp(http_option_data_,"\r\n") == 0) {
printf("%s\n",http_option_data_);
memset(&http_option_data_[0],0,MAX_HTTP_LINE_SIZE);
}
if (pipe(handle_in_pipe_) < 0 || pipe(handle_out_pipe_) < 0)
_clear_data();
int pid = fork();
if (pid < 0)
_clear_data();
if (pid == 0) {
//
close(handle_in_pipe_[0]);
close(handle_out_pipe_[1]);
dup2(handle_in_pipe_[1],1); //associate wr-pipe with stdout
dup2(handle_out_pipe_[0],0); //associate rd-pipe with stdin
//set up env
char env_buff[MAX_HTTP_FIELD_VALUE_LEN];
for (int i = 0; i < MAX_HTTP_FIELD_PAIR_NUM; i++) {
if (http_arg_info_[i].key) {
memset(&env_buff[0],0,sizeof(env_buff));
sprintf(env_buff,"%s=%s",http_arg_info_[i].key,http_arg_info_[i].value);
putenv(&env_buff[0]);
// fprintf(stderr,"get env %s\t%s",http_arg_info_[i].key,getenv(http_arg_info_[i].key));
}
}
if (execl("./cgi.sh","test",NULL) < 0)
printf("can not load exe file,please check the excute previlege");
}
else if (pid > 0 ) {
close(handle_in_pipe_[1]);
close(handle_out_pipe_[0]);
//handle data
//only for read
char rdchar;
while (read(handle_in_pipe_[0],&rdchar,1))
fwrite(&rdchar,1,1,sock_file_);
//release fd
close(handle_in_pipe_[0]);
close(handle_out_pipe_[1]);
//waiting for child process exit
int status = 0;
waitpid(pid, &status, 0);
_clear_data();
}
}