void main(void) {
//configuring
P1OUT |= 0x04; // set P1.2 for debug
P4DIR |= 0x20; // P4.5 as output (for debug)
gina_init();
scheduler_init();
leds_init();
if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation)
gyro_init();
large_range_accel_init();
magnetometer_init();
sensitive_accel_temperature_init();
}
radio_init();
timer_init();
P1OUT &= ~0x04; // clear P1.2 for debug
//check sensor configuration is right
gyro_get_config();
large_range_accel_get_config();
magnetometer_get_config();
sensitive_accel_temperature_get_config();
//scheduler_push_task(ID_TASK_APPLICATION);
scheduler_register_application_task(&task_application_imu_radio, 0, FALSE);
scheduler_start();
}
int main(int argc, char **argv)
{
/* 模块初始化 */
exc_init(); /* 中断初始化 */
sys_timer_init(); /* 系统时钟初始化 */
light_init(); /* LED灯初始化 */
switch_init(); /* 开关初始化 */
speaker_init(); /* 蜂鸣器初始化 */
motor_init(); /* 电机初始化 */
decoder_init(); /* 编码器初始化 */
gyro_init(); /* 陀螺仪初始化 */
acc_init(); /* 加速度传感器初始化 */
serial_initialize((intptr_t)(NULL)); /* 初始化串口 */
//sd_init(&Fatfs); /* 初始化SD卡，并创建文件 */
//sd_create_file(&test_data, test_data_name);
/* 命令注册 */
help_cmd_initialize((intptr_t)(NULL));
light_cmd_initialize((intptr_t)(NULL));
switch_cmd_initialize((intptr_t)(NULL));
speaker_cmd_initialize((intptr_t)(NULL));
motor_cmd_initialize((intptr_t)(NULL));
decoder_cmd_initialize((intptr_t)(NULL));
//sd_cmd_initialize((intptr_t)(NULL));
printf("\n Welcome to k60 software platform!");
printf("\n Press 'help' to get the help! \n");
light_open(LIGHT4);
/* ntshell测试 */
task_ntshell((intptr_t)(NULL));
}
static int
gyroscope_l3g4200d_open(struct sol_flow_node *node,
void *data,
const struct sol_flow_node_options *options)
{
struct gyroscope_l3g4200d_data *mdata = data;
const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts =
(const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options;
SOL_NULL_CHECK(options, -EINVAL);
mdata->i2c = sol_i2c_open(opts->i2c_bus.val, I2C_SPEED);
if (!mdata->i2c) {
SOL_WRN("Failed to open i2c bus");
return -EBADR;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n",
GYRO_ADDRESS);
return -EIO;
}
mdata->use_rad = opts->output_radians;
mdata->init_sampling_cnt = 3;
mdata->ready = false;
mdata->node = node;
return gyro_init(mdata);
}
void init()
{
bSmartDiagnostics = true; //true to enable smart diagnostic screen
bCompetitionMode = true; //true to enable competition mode
displaySplash("Mecanum Bot", "", true);
bool ok = false;
while(!ok)
{
const int testcount = 2;
bool test[testcount] = {
errorcheck(1,0,1,MOTORCON),
errorcheck(1,0,2,MOTORCON)};
string desc[testcount] = {"MC1-1","MC1-2"};
ok = error_display(test,desc,testcount);
if (!ok) {
PlayTone(440, 50);
if (test[0] == false && test[1] == false){
nxtDisplayCenteredTextLine(7, "Reboot MC!");
}
}
else { ClearSounds(); }
}
eraseDisplay();
gyro_init(HTGYRO);
wait1Msec(50);
nxtbarOn();
return;
}
int usetup (void) {
extern volatile uint8_t robot_id;
robot_id = 8;
territory_init();
gyro_init(PIN_GYRO, LSB_US_PER_DEG, 500);
return 0;
}
// usetup is called during the calibration period. It must return before the
// period ends.
int usetup (void) {
printf("\nPlace robot, press go.");
go_click ();
printf ("\nStabilizing...");
pause (500);
printf ("\nCalibrating offset...\n");
gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
return 0;
}
int usetup (void) {
extern volatile uint8_t robot_id;
robot_id = 8;
gyro_init(PIN_GYRO, LSB_US_PER_DEG, 500);
printf("starting daemons...");
// vps_data_daemon_init();
hybrid_position_daemon_init();
printf(" done\n");
return 0;
}
void init()
{
bSmartDiagnostics = true; //true to enable smart diagnostic screen
bCompetitionMode = false; //true to enable competition mode
displaySplash("Mecanum Bot", "FOD Test", true);
eraseDisplay();
gyro_init(HTGYRO);
wait1Msec(50);
nxtbarOn();
return;
}
void i2cdriver_init(void) {
i2cStart(&I2CD1, &i2c_conf); // Start the I2C1 driver
debug_println("Init: I2C Driver Active");
accel_init(); // Prepare the accelerometer
debug_println("Init: Accelerometer Active");
chThdSleepMilliseconds(100); // May not be needed. Using as in example code.
gyro_init(); // Prepare the gyroscope
debug_println("Init: Gyroscope Active");
chThdSleepMilliseconds(100); // Also may not be needed.
}
imu::imu(void)
{ twi_init(); // Initialize all peripherals.
accl_init();
mag_init();
gyro_init();
gyro_roll_angle = 0; // Initialize internal variables to zero.
gyro_pitch_angle = 0;
roll_angle = 0;
pitch_angle = 0;
yaw_angle = 0;
//gyro_x_position(void)
}
int usetup (void) {
printf("\nPlace robot, press go.");
go_click ();
printf ("\nStabilizing...");
pause (500);
printf ("\nCalibrating offset...\n");
gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
extern volatile uint8_t robot_id;
robot_id = 128;
extern volatile uint8_t light_port;
light_port = 4;
return 0;
}
void init (void)
{
//1 = output, 0 = input
DDRB = 0b01100000; //PORTB4, B5 output for stat LED
DDRC = 0b00010000; //PORTC4 (SDA), PORTC5 (SCL), PORTC all others are inputs
DDRD = 0b00000010; //PORTD (TX output on PD1)
PORTC = 0b00110000; //pullups on the I2C bus
cbi(PORTB, 5);
i2cInit();
accelerometer_init();
magnetometer_init();
gyro_init();
check_baud();
}
void main(void)
{
WDTCTL = WDTPW + WDTHOLD; // disable watchdog timer
BCSCTL1 = CALBC1_16MHZ; // MCLK at 16MHz
DCOCTL = CALDCO_16MHZ;
P1DIR |= 0x1E; // P1.1-4 as outputs (for debug)
__enable_interrupt(); // global enable interrupts
leds_init();
//configuring the gyro
if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation)
i2c_init():
gyro_init();
}
void setup_state() {
while (state == SETUP) {
create_thread(sing, 64, 127, "sing_thread");
printf("\nPress go");
go_click();
printf("\nStabilizing");
pause(1000);
printf("\nInitializing");
gyro_init(8,1357.348162*3838.0/3600.0*1028.0/1080.0,5000);
target_angle = 0;
reset_pid_controller(target_angle);
setup_filter();
}
}
void init()
{
servo[TubeWinch] = 127;
servo[GoalRetainer] = 5;
servo[Kickstand] = 155;
servo[BallStorage] = 80;
servo[TouchSensor] = 65;
bSmartDiagnostics = true; //true to enable smart diagnostic screen
bCompetitionMode = true; //true to enable competition mode
log_enabled = true; //Enable logging in Teleop
displaySplash("High PHidelity", "Teleop", true);
eraseDisplay();
gyro_init(HTGYRO);
wait1Msec(50);
nxtbarOn();
return;
}
int usetup (void) {
extern volatile uint8_t robot_id;
robot_id = 8;
printf("stabilizing... ");
pause(700);
printf("done\n");
territory_init();
printf("calibrating gyro... ");
gyro_init(PIN_GYRO, LSB_US_PER_DEG, 1000);
printf("done\n");
vps_data_daemon_init();
pause(300);
printf("binding gyro to vps coordinates... ");
gyro_set_degrees(get_vps_theta());
printf("done\n");
determine_team_color();
return 0;
}
void board_init_devices(void) {
#if defined(ENVIRONMENT_SENSOR)
kputs("Initializing i2c\n");
i2c_init();
kputs("Initializing Devices... Light,");
light_init();
#ifdef REPORT_TYPE_GYRO
kputs("Gyro, ");
gyro_init();
#endif
#ifdef REPORT_TYPE_ACCEL
kputs("Accell, ");
accel_init();
#endif
#ifdef REPORT_TYPE_HUMID
kputs("Humid, ");
humid_init();
#endif
kputs("PIR\n");
pir_init();
#elif defined POWER_STRIP_MONITOR
kputs("Initializing Devices... Power\n");
powermon_init();
#endif
#ifdef USE_PN532
kputs("Initializing RFID reader... \n");
rfid_init();
#endif
#ifdef USE_MACHXO2_PMCO2
kputs("Initializing CO2 and PM extension...\n");
machxo2_init();
#endif
#ifdef USE_DOOR_SENSORS
kputs("Initializing Door Sensors...\n");
door_sensors_init();
#endif
}
int
sensors_init(void)
{
int ret;
int ret_combined = 0;
ret = accel_init();
if (ret) { ret_combined = ret; }
ret = gyro_init();
if (ret) { ret_combined = ret; }
ret = mag_init();
if (ret) { ret_combined = ret; }
ret = baro_init();
if (ret) { ret_combined = ret; }
return ret_combined;
}
static int
gyroscope_l3g4200d_open(struct sol_flow_node *node,
void *data,
const struct sol_flow_node_options *options)
{
struct gyroscope_l3g4200d_data *mdata = data;
const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts =
(const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options;
SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options,
SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D_OPTIONS_API_VERSION,
-EINVAL);
mdata->i2c = sol_i2c_open(opts->i2c_bus, I2C_SPEED);
SOL_NULL_CHECK_MSG(mdata->i2c, -EIO, "Failed to open i2c bus");
mdata->use_rad = opts->output_radians;
mdata->init_sampling_cnt = 3;
mdata->node = node;
gyro_init(mdata);
return 0;
}
static void
i2c_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD : GYRO_SCALE_R_S;
uint8_t num_samples_available;
struct sol_direction_vector val =
{
.min = -GYRO_RANGE,
.max = GYRO_RANGE,
.x = mdata->reading[0],
.y = mdata->reading[1],
.z = mdata->reading[2]
};
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("Failed to read L3G4200D gyro fifo status");
return;
}
num_samples_available = status / (sizeof(int16_t) * 3);
/* raw readings, with only the sensor-provided filtering */
for (uint8_t i = 0; i < num_samples_available; i++) {
mdata->reading[0] = mdata->gyro_data.buffer[i][0] * scale;
mdata->reading[1] = -mdata->gyro_data.buffer[i][1] * scale;
mdata->reading[2] = -mdata->gyro_data.buffer[i][2] * scale;
}
sol_flow_send_direction_vector_packet(mdata->node,
SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT, &val);
mdata->pending_ticks--;
if (mdata->pending_ticks)
gyro_tick_do(mdata);
}
static void
i2c_read_fifo_status_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
uint8_t num_samples_available;
uint8_t fifo_status = mdata->common.buffer[0];
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("Failed to read L3G4200D gyro fifo status");
return;
}
if (fifo_status & GYRO_REG_FIFO_SRC_OVERRUN) {
num_samples_available = 32;
} else if (fifo_status & GYRO_REG_FIFO_SRC_EMPTY) {
num_samples_available = 0;
} else {
num_samples_available = fifo_status
& GYRO_REG_FIFO_SRC_ENTRIES_MASK;
}
if (!num_samples_available) {
SOL_INF("No samples available");
return;
}
SOL_DBG("%d samples available", num_samples_available);
/* Read *all* the entries in one go, using AUTO_INCREMENT */
mdata->i2c_pending = sol_i2c_read_register(mdata->i2c,
GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT,
(uint8_t *)&mdata->gyro_data.buffer[0][0],
sizeof(mdata->gyro_data.buffer), i2c_read_data_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("Failed to read L3G4200D gyro samples");
}
static bool
gyro_tick_do(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_tick_do);
return false;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
return false;
}
mdata->common.buffer[0] = 0;
mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_FIFO_SRC,
mdata->common.buffer, 1, i2c_read_fifo_status_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("Failed to read L3G4200D gyro fifo status");
return false;
}
static bool
gyro_ready(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->ready = true;
SOL_DBG("gyro is ready for reading");
if (mdata->pending_ticks)
gyro_tick_do(mdata);
return false;
}
static void
i2c_write_fifo_ctl_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("could not set L3G4200D gyro sensor's stream mode");
return;
}
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_ready) < 0)
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
}
static bool
gyro_init_stream(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream);
return false;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
return false;
}
/* enable FIFO in stream mode */
mdata->common.buffer[0] = GYRO_REG_FIFO_CTL_STREAM;
mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL,
mdata->common.buffer, 1, i2c_write_fifo_ctl_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("could not set L3G4200D gyro sensor's stream mode");
return false;
}
static void
i2c_write_ctrl_reg5_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("could not set L3G4200D gyro sensor's fifo mode");
return;
}
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME,
gyro_init_stream) < 0)
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
}
static bool
gyro_init_fifo(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo);
return false;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
return false;
}
mdata->common.buffer[0] = GYRO_REG_CTRL_REG5_FIFO_EN;
mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5,
mdata->common.buffer, 1, i2c_write_ctrl_reg5_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("could not set L3G4200D gyro sensor's fifo mode");
return false;
}
static void
i2c_write_ctrl_reg4_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("could not set L3G4200D gyro sensor's resolution");
return;
}
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME,
gyro_init_fifo) < 0)
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
}
static bool
gyro_init_range(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_range);
return false;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
return false;
}
/* setup for 2000 degrees/sec */
mdata->common.buffer[0] = GYRO_REG_CTRL_REG4_FS_2000;
mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4,
mdata->common.buffer, 1, i2c_write_ctrl_reg4_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("could not set L3G4200D gyro sensor's resolution");
return false;
}
static bool gyro_init_sampling(void *data);
static void
i2c_write_ctrl_reg1_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("could not set L3G4200D gyro sensor's sampling rate");
return;
}
mdata->init_sampling_cnt--;
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME,
mdata->init_sampling_cnt ?
gyro_init_sampling : gyro_init_range) < 0) {
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
}
}
/* meant to run 3 times */
static bool
gyro_init_sampling(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling);
return false;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
return false;
}
/* setup for 800Hz sampling with 110Hz filter */
mdata->common.buffer[0] = GYRO_REG_CTRL_REG1_DRBW_800_110 |
GYRO_REG_CTRL_REG1_PD | GYRO_REG_CTRL_REG1_XYZ_ENABLE;
mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1,
mdata->common.buffer, 1, i2c_write_ctrl_reg1_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("could not set L3G4200D gyro sensor's sampling rate");
return false;
}
static void
i2c_read_who_am_i_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status)
{
struct gyroscope_l3g4200d_data *mdata = cb_data;
mdata->i2c_pending = NULL;
if (status < 0) {
SOL_WRN("Failed to read i2c register");
return;
}
if (mdata->common.buffer[0] != GYRO_REG_WHO_AM_I_VALUE) {
SOL_WRN("could not find L3G4200D gyro sensor");
return;
}
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling);
}
static bool
gyro_init(void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init);
return false;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n",
GYRO_ADDRESS);
return false;
}
mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_WHO_AM_I,
mdata->common.buffer, 1, i2c_read_who_am_i_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("Failed to read i2c register");
return false;
}
static int
gyroscope_l3g4200d_open(struct sol_flow_node *node,
void *data,
const struct sol_flow_node_options *options)
{
struct gyroscope_l3g4200d_data *mdata = data;
const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts =
(const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options;
SOL_NULL_CHECK(options, -EINVAL);
mdata->i2c = sol_i2c_open(opts->i2c_bus, I2C_SPEED);
SOL_NULL_CHECK_MSG(mdata->i2c, -EIO, "Failed to open i2c bus");
mdata->use_rad = opts->output_radians;
mdata->init_sampling_cnt = 3;
mdata->node = node;
gyro_init(mdata);
return 0;
}
static void
gyroscope_l3g4200d_close(struct sol_flow_node *node, void *data)
{
struct gyroscope_l3g4200d_data *mdata = data;
if (mdata->i2c_pending)
sol_i2c_pending_cancel(mdata->i2c, mdata->i2c_pending);
if (mdata->i2c)
sol_i2c_close(mdata->i2c);
if (mdata->timer)
sol_timeout_del(mdata->timer);
}
static int
gyroscope_l3g4200d_tick(struct sol_flow_node *node,
void *data,
uint16_t port,
uint16_t conn_id,
const struct sol_flow_packet *packet)
{
struct gyroscope_l3g4200d_data *mdata = data;
if (!mdata->ready || mdata->pending_ticks) {
mdata->pending_ticks++;
return 0;
}
gyro_tick_do(mdata);
return 0;
}
int usetup(void){
robot_id = 1; //team 1
gyro_init(GYRO_PORT, GYRO_DEGREE_CONVERSION, GYRO_CALIB_WAIT_TIME);
return 0;
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
mcu_init();
sys_time_init();
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_INFRARED
infrared_init();
#endif
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#ifdef USE_IMU
imu_init();
#endif
#ifdef USE_AHRS
ahrs_aligner_init();
ahrs_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#ifdef MODEM
modem_init();
#endif
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
estimator_init();
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
nav_init();
modules_init();
settings_init();
/** - start interrupt task */
mcu_int_enable();
/** wait 0.5s (historical :-) */
sys_time_usleep(500000);
#if defined GPS_CONFIGURE
gps_configure_uart();
#endif
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
power_switch = FALSE;
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
void
Sensors::task_main()
{
/* start individual sensors */
accel_init();
gyro_init();
mag_init();
baro_init();
adc_init();
/*
* do subscriptions
*/
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
/*
* do advertisements
*/
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
raw.adc_voltage_v[0] = 0.0f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
diff_pres_poll(raw);
parameter_update_poll(true /* forced */);
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* wakeup source(s) */
struct pollfd fds[1];
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
fds[0].fd = _gyro_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 50ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* check parameters for updates */
parameter_update_poll();
/* the timestamp of the raw struct is updated by the gyro_poll() method */
/* copy most recent sensor data */
gyro_poll(raw);
accel_poll(raw);
mag_poll(raw);
baro_poll(raw);
/* check battery voltage */
adc_poll(raw);
diff_pres_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing) {
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
}
/* Look for new r/c input data */
rc_poll();
perf_end(_loop_perf);
}
warnx("[sensors] exiting.");
_sensors_task = -1;
_exit(0);
}
int main(int argc, char *argv[]) {
data_t accel_data, gyro_data;
data_t gyro_offset;
float a_res, g_res;
mraa_i2c_context accel, gyro;
accel_scale_t a_scale = A_SCALE_8G;
gyro_scale_t g_scale = G_SCALE_2000DPS;
int client_socket_fd, portno, n;
struct sockaddr_in serv_addr;
struct hostent *server;
int flags;
float buffer[BUFF_SIZE];
int send_idx = 0, curr_idx = 0;
int next_it, send_it, curr_it; // helper variables for converting idx (byte) to it (float)
int num_pts = 0;
int i;
int init_sam = (SAMP_RATE*INIT_SEC)+1; // number of samples during init period
int quit = 0; // quit flag
char stop; // dummy buffer for stop ping
// Read command line arguments, need to get the host IP address and port
if (argc < 3) {
fprintf(stderr, "USAGE: %s <hostname> <port>\n", argv[0]);
exit(1);
}
// Convert the arguments to the appropriate data types
portno = atoi(argv[2]);
// setup the socket
// technically PF_INET. Refer to:
// http://stackoverflow.com/questions/6729366/what-is-the-difference-between-af-inet-and-pf-inet-in-socket-programming
//client_socket_fd = socket(AF_INET, SOCK_STREAM, 0);
client_socket_fd = socket(PF_INET, SOCK_STREAM, 0);
// check if the socket was created successfully. If it wasnt, display an error and exit
if(client_socket_fd < 0) {
error("ERROR opening socket");
}
// check if the IP entered by the user is valid
server = gethostbyname(argv[1]);
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(1);
}
// clear our the serv_addr buffer
memset((char *) &serv_addr, 0, sizeof(serv_addr));
// set up the socket
serv_addr.sin_family = AF_INET;
memcpy((char *)&serv_addr.sin_addr.s_addr, (char *)server->h_addr, server->h_length);
serv_addr.sin_port = htons(portno);
// try to connect to the server
if (connect(client_socket_fd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0){
error("ERROR connecting");
}
// make non-blocking. Refer to:
// http://developerweb.net/viewtopic.php?id=3000
/* Set socket to non-blocking */
if ((flags = fcntl(client_socket_fd, F_GETFL, 0)) < 0) {
error("ERROR getting socket flags");
}
if (fcntl(client_socket_fd, F_SETFL, flags | O_NONBLOCK) < 0) {
error("ERROR setting socket to non-blocking");
}
printf("Socket connected to server!\n");
printf("Now beginning sensor initialization\n");
// initialize sensors, set scale, and calculate resolution.
accel = accel_init();
set_accel_scale(accel, a_scale);
a_res = calc_accel_res(a_scale);
gyro = gyro_init();
set_gyro_scale(gyro, g_scale);
g_res = calc_gyro_res(g_scale);
gyro_offset = calc_gyro_offset(gyro, g_res);
printf("GYRO OFFSETS x: %f y: %f z: %f\n",
gyro_offset.x, gyro_offset.y, gyro_offset.z);
//Read the sensor data and print them.
printf("STARTING TO COLLECT DATA\n");
printf("\n \t\tAccelerometer\t\t\t||");
printf("\t\t\tGyroscope\t\t\t\n");
while (1) {
for (i = 0; i < PACK_PTS; i++) {
n = read(client_socket_fd, &stop, 1);
if (n < 0) {
if (!(errno == EWOULDBLOCK || errno == EAGAIN))
error("ERROR reading from client socket");
// else no stop ping yet
} else {
#ifdef DEBUG
printf("RECEIVED STOP PING\n");
#endif
quit = 1;
break;
}
if (num_pts == init_sam) printf("INITIALIZATION PERIOD DONE\n");
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
if (num_pts%PRINT_PERIOD == 0) {
printf("%i points\n", num_pts);
printf(" send_idx: %d curr_idx: %d\n", send_idx, curr_idx);
printf(" X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z);
printf("\tX: %f\t Y: %f\t Z: %f p: %d\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts);
}
#ifdef DEBUG
printf("X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z);
printf("\tX: %f\t Y: %f\t Z: %f p: %d\t\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts);
#endif
curr_it = curr_idx/sizeof(float);
buffer[curr_it] = gyro_data.x - gyro_offset.x;
buffer[curr_it+1] = gyro_data.y - gyro_offset.y;
buffer[curr_it+2] = gyro_data.z - gyro_offset.z;
buffer[curr_it+3] = accel_data.x;
buffer[curr_it+4] = accel_data.y;
buffer[curr_it+5] = accel_data.z;
num_pts++;
next_it = (curr_it + SAMP_SIZE)%BUFF_SIZE;
send_it = send_idx/sizeof(float); // truncate integer
if ((next_it >= send_it) &&
(next_it <= (send_it+SAMP_SIZE-1))) {
// note that curr_idx always advances SAMP_SIZE at a time
// went around ring buffer. panic!
fprintf(stderr, "Ring buffer is full. Abort!\n");
exit(1);
}
curr_idx = next_it*sizeof(float);
} // batch done
// stop writing packets if received stop ping
if (quit) break;
// write batch to server
n = write(client_socket_fd, ((char*)buffer)+send_idx,
((curr_idx>send_idx)?
(curr_idx-send_idx):
(BUFF_BYTES-send_idx)));
// n contains how many bytes were written to the socket
// if n is less than 0, then there was an error
// Refer to:
// http://stackoverflow.com/questions/3153939/properly-writing-to-a-nonblocking-socket-in-c
// http://beej.us/guide/bgnet/output/html/singlepage/bgnet.html
if (n < 0) {
if (errno == EWOULDBLOCK || errno == EAGAIN) {
fprintf(stderr, "FAILED WRITE for byte %d. Currently at byte %d\n",
send_idx, curr_idx);
} else {
error("ERROR writing to socket");
}
} else { // successful write
send_idx = (send_idx+n)%BUFF_BYTES;
}
#ifdef DEBUG
printf("send_idx: %d curr_idx: %d\n", send_idx, curr_idx);
#endif
usleep(1000000/SAMP_RATE);
}
close(client_socket_fd);
return 0;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data;
int16_t temperature;
float pitch_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char *posture_message;
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno, n;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
int count;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
portno = 41234;
//create socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, &pitch_angle, &yaw_angle);
printf("is moving: %d\n", isMoving(gyro_data) );
//printf("yaw angle: %f ", yaw_angle);
//printf("pitch angle: %f ", pitch_angle);
//printf("z vector: %f\n", accel_data.z);
if (count == 3) {
//send posture to cloud
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, yaw_angle);
else
curr_posture = prev_posture; //else just use the old posture
if (curr_posture==UNDEFINED)
curr_posture = prev_posture;
prev_posture = curr_posture; //set new value for prev posture
posture_message = construct_message(POS, accel_data, curr_posture);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
//store accel data in string
/*
x_accel_message = construct_message(X_DIR, accel_data);
//printf("%s\n", full_message_x);
//send UDP message
n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
y_accel_message = construct_message(Y_DIR, accel_data);
//printf("%s\n", full_message_y);
n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
z_accel_message = construct_message(Z_DIR, accel_data);
//printf("%s\n", full_message_z);
n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
*/
count = 0;
}
char *posture_string;
switch(curr_posture)
{
case 0:
posture_string = "upright";
break;
case 1:
posture_string = "face up";
break;
case 2:
posture_string = "face down";
break;
case 3:
posture_string = "left";
break;
case 4:
posture_string = "right";
break;
default:
posture_string = "undefined";
}
printf("current orientation: %s \n", posture_string);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
/*******************************************************************
* MAIN()
*******************************************************************/
int
main(void)
{
long lEEPROMRetStatus;
uint16_t i=0;
uint8_t halted_latch = 0;
// Set the clocking to run at 80 MHz from the PLL.
// (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't
// write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we
// could slow the clock down when writing to FLASH but then we need to find out how long
// it takes for the clock to stabilize. This is on at the bottom of my list of things to do
// for now)
SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
// Initialize the device pinout.
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ);
// Enable processor interrupts.
IntMasterEnable();
// Setup the UART's
my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// command_handler_init overwrites the baud rate. We still need to configure the pins though
my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// Enable the command handler
command_handler_init(); // We set the baud in here
// Start the timers
my_timer0_init();
my_timer1_init();
i2c_init();
motor_init();
qei_init();
gyro_init();
accel_init();
led_init();
//rc_radio_init();
//setupBluetooth();
// Initialize the EEPROM emulation region.
lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE);
if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14);
#if 0
// If ever we wanted to write some parameters to FLASH without the HMI
// we could do it here.
SoftEEPROMWriteDouble(kP_ID, 10.00);
SoftEEPROMWriteDouble(kI_ID, 10.00);
SoftEEPROMWriteDouble(kD_ID, 10.00);
SoftEEPROMWriteDouble(ANG_ID, 0.0);
SoftEEPROMWriteDouble(COMPC_ID, 0.99);
#endif
kP = SoftEEPROMReadDouble(kP_ID);
kI = SoftEEPROMReadDouble(kI_ID);
kD = SoftEEPROMReadDouble(kD_ID);
commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID);
COMP_C = SoftEEPROMReadDouble(COMPC_ID);
pid_init(kP, kI, kD, &pid_ang);
motor_controller_init(20, 100, 10, &mot_left);
motor_controller_init(20, 100, 10, &mot_right);
//pid_init(0.0, 0.0, 0.0, &pid_pos_left);
//pid_init(0.0, 0.0, 0.0, &pid_pos_right);
//UART0Send("Hello World!\n", 13);
// Tell the HMI what the initial parameters are.
print_params(1);
while(1)
{
delta_t = myTimerValueGet();
myTimerZero();
sum_delta_t += delta_t;
// Read our sensors
accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true);
gyro_get_y_cal(&gyro_y, false);
// Calculate the pitch angle with the accelerometer only
R = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang;
//accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0);
gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t);
// Kalman filter
//filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t));
filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang);
// Skip the rest of the process until the angle stabilizes
if(i < 250) { i++; continue; }
// Tell the HMI what's going on every 100ms
if(sum_delta_t >= 1000)
{
print_update(1);
print_debug(0);
//print_control_surfaces(0);
led_toggle();
//print_angle();
sum_delta_t = 0;
}
// See if the HMI has anything to say
command_handler();
//continue;
// If we are leaning more than +/- FALL_ANG deg off center it's hopeless.
// Turn off the motors in hopes of some damage control
if( abs(filtered_ang) > FALL_ANG )
{
if(halted_latch) continue;
stop_motors();
halted_latch = 1;
continue;
}
halted_latch = 0;
motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang);
motor_left = motor_right = motor_val;
drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain);
}
}
int usetup (void) {
robot_id = 10;
gyro_init(GYRO_PORT, LSB_US_PER_DEG, 500L);
return 0;
}
int usetup (void) {
gyro_init(PIN_GYRO, LSB_US_PER_DEG, 500);
return 0;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data, zero_rate;
int16_t temperature;
float pitch_angle, roll_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char posture_message[20];
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
char serv_message[256];
int count;
int n, n1;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
zero_rate = calc_gyro_offset(gyro, g_res);
portno = 2015;
//create socket
if (argc > 1) //if user supplies IP address as argument
upperbodyIP = argv[1]; //take user input as upper body IP
else
upperbodyIP = "192.168.0.30";
sockfd = socket(AF_INET, SOCK_STREAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname(upperbodyIP); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//WAIT FOR THE OKAY BY THE SERVER//
memset(serv_message, 0, 256);
n = read(sockfd, serv_message, 256);
if (strcmp(serv_message, "START") != 0)
error("strange response from server");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, gyro_data, zero_rate, &pitch_angle, &roll_angle, &yaw_angle);
printf("is moving: %d ", isMoving(gyro_data));
printf("yaw angle: %f ", yaw_angle);
if (count == 3) {
//send posture to main edison
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, roll_angle);
else
curr_posture = UNDEFINED; //else just use the undefined/transition case
prev_posture = curr_posture; //set new value for prev posture
memset(posture_message, 0, sizeof(char)*20);
snprintf(posture_message, 10, "%d,%f", curr_posture, yaw_angle);
//posture_message = construct_message(POS, accel_data, curr_posture);
printf("posture message: %s ", posture_message);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
/*
bzero(message, 256);
printf("waiting for response ");
n1 = read(sockfd, message, 10);
if (n1 < 0)
error("ERROR reading from upper body");
printf("got response\n");
*/
count = 0;
}
printPostureString(curr_posture);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
hw_init();
sys_time_init();
#ifdef LED
led_init();
#endif
#ifdef ADC
adc_init();
#endif
#endif /* SINGLE_MCU */
/************* Sensors initialization ***************/
#ifdef USE_INFRARED
ir_init();
#endif
#ifdef USE_GYRO
gyro_init();
#endif
#ifdef USE_GPS
gps_init();
#endif
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef USE_UART2
Uart2Init();
#endif
#ifdef USE_UART3
Uart3Init();
#endif
#ifdef USE_USB_SERIAL
VCOM_init();
#endif
#ifdef USE_GPIO
GpioInit();
#endif
#ifdef USE_I2C0
i2c0_init();
#endif
#ifdef USE_I2C1
i2c1_init();
#endif
#ifdef USE_I2C2
i2c2_init();
#endif
/************* Links initialization ***************/
#if defined USE_SPI
spi_init();
#endif
#if defined MCU_SPI_LINK
link_mcu_init();
#endif
#ifdef MODEM
modem_init();
#endif
/************ Internal status ***************/
h_ctl_init();
v_ctl_init();
estimator_init();
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
nav_init();
modules_init();
/** - start interrupt task */
int_enable();
/** wait 0.5s (historical :-) */
sys_time_usleep(500000);
#if defined GPS_CONFIGURE
gps_configure_uart();
#endif
#if defined DATALINK
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif
power_switch = FALSE;
/************ Multi-uavs status ***************/
#ifdef TRAFFIC_INFO
traffic_info_init();
#endif
}
