void Replay::loop() { char type[5]; if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) { if (!hal.util->get_soft_armed()) { hal.util->set_soft_armed(true); ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis()); } } if (!logreader.update(type)) { ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f); flush_and_exit(); } if (last_timestamp != 0) { uint64_t gap = AP_HAL::micros64() - last_timestamp; if (gap > 40000) { ::printf("Gap in log at timestamp=%lu of length %luus\n", last_timestamp, gap); } } last_timestamp = AP_HAL::micros64(); read_sensors(type); if (!streq(type,"ATT")) { return; } }
int main(int argc, char **argv) { ros::init(argc, argv, "imu"); ros::NodeHandle nh; ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000); std_msgs::Float64 msg; ros::Rate loopRate(10); int temp = 0; removegyrooff(); while (ros::ok()) { read_sensors(); msg.data = TO_DEG(-atan2(magnetom[1], magnetom[0])); chatter_pub.publish(msg); ROS_INFO("%s %f", "send an imu message", TO_DEG(-atan2(magnetom[1], magnetom[0]))); // ROS_INFO("%d \n",temp); ros::spinOnce(); loopRate.sleep(); temp++; } }
void readImu() { timestamp_old = timestamp; timestamp = millis(); if (timestamp > timestamp_old) G_Dt = (float)(timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; // Update sensor readings read_sensors(); if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) // We're in calibration mode { check_reset_calibration_session(); // Check if this session needs a reset } else if (output_mode == OUTPUT__MODE_ANGLES) // Output angles { // Apply sensor calibration compensate_sensor_errors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); } }
// Read every sensor and record a time stamp // Init DCM with unfiltered orientation // TODO re-init global vars? void reset_sensor_fusion() { float temp1[3]; float temp2[3]; float xAxis[] = { 1.0f, 0.0f, 0.0f }; read_sensors(); timestamp = millis(); // GET PITCH // Using y-z-plane-component/x-component of gravity vector pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); // GET ROLL // Compensate pitch of gravity vector Vector_Cross_Product(temp1, accel, xAxis); Vector_Cross_Product(temp2, xAxis, temp1); // Normally using x-z-plane-component/y-component of compensated gravity vector // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2])); // Since we compensated for pitch, x-z-plane-component equals z-component: roll = atan2(temp2[1], temp2[2]); // GET YAW Compass_Heading(); yaw = MAG_Heading; // Init rotation matrix init_rotation_matrix(DCM_Matrix, yaw, pitch, roll); }
static int monitor_task(void *arg) { struct thermostat* th = arg; while(!kthread_should_stop()) { if (unlikely(freezing(current))) refrigerator(); msleep_interruptible(2000); #ifdef DEBUG DumpTachoRegisters(); #endif read_sensors(th); update_fan_speed(th); #ifdef DEBUG /* be carefule with the stats displayed. The Fan Counter value depends * on what value is written in the register during the read sensors * call. If its in temperature read setting, the fan counter and hence * the rpm will be WRONG */ display_stats(th); #endif } return 0; }
void loop() { read_sensors(); short direction = drive.forward; int speed = clamp( vertical.value(), drive.v_center - drive.v_gap - drive.v_range, drive.v_center + drive.v_gap + drive.v_range); // speed is from 0 to 100 if (speed > drive.v_center + drive.v_gap) { speed = (speed - (drive.v_center + drive.v_gap)) / 4; direction = drive.backward; } else if (speed < drive.v_center - drive.v_gap) { speed = (drive.v_center - drive.v_gap - speed) / 4; } else { speed = 0; } // side is between -100 and 100 // -100 is all the way on the right int side = clamp( horizontal.value(), drive.h_center - drive.h_gap - drive.h_range, drive.h_center + drive.h_gap + drive.h_range); if (side > drive.h_center + drive.h_gap) { side = (side - (drive.h_center + drive.h_gap)) / 4; } else if (side < (drive.h_center - drive.h_gap)) { side = (side - (drive.h_center - drive.h_gap)) / 4; } else { side = 0; } // allow more careful controls while turning short min_speed = -drive.turn_assist * direction; short max_speed = speed + (drive.turn_assist * direction); // get the left/right speed -- never more than limits long right = direction * clamp(speed - side, min_speed, max_speed); long left = direction * clamp(speed + side, min_speed, max_speed); // proportional to the max speed we want sabertooth to go left = drive.max_speed * left / 100; right = drive.max_speed * right / 100; send_velocity_to_computer(speed, side, left, right); send_velocity_to_sabertooth(left, right); // toggle the run led every time toggle_led(); }
/** Turn left at an intersection. */ void turn_left(){ buzzer_off(); motion_set(0x05); velocity(100,100); _delay_ms(1000); while(1){ print_sensor_data(); read_sensors(); if(Center_white_line < W_THRESHOLD) break; } velocity(0,0); }
void tx_thread(void) { while(1) { send_buf.size = read_sensors(&send_buf); com_send(IFACE_RADIO, &send_buf); printf("[TX] sent %d bytes\n", send_buf.size); mos_led_toggle(0); mos_thread_sleep(5000); } }
void hi_reload(gint entry) { switch (entry) { case COMPUTER_FILESYSTEMS: scan_filesystems(); break; case COMPUTER_NETWORK: scan_net_interfaces(); break; case COMPUTER_SENSORS: read_sensors(); break; } }
static Computer * computer_get_info(void) { Computer *computer; computer = g_new0(Computer, 1); if (moreinfo) { #ifdef g_hash_table_unref g_hash_table_unref(moreinfo); #else g_free(moreinfo); #endif } moreinfo = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL); shell_status_update("Getting processor information..."); computer->processor = computer_get_processor(); shell_status_update("Getting memory information..."); computer->memory = computer_get_memory(); shell_status_update("Getting operating system information..."); computer->os = computer_get_os(); shell_status_update("Getting display information..."); computer->display = computer_get_display(); shell_status_update("Getting sound card information..."); computer->alsa = computer_get_alsainfo(); shell_status_update("Getting mounted file system information..."); scan_filesystems(); shell_status_update("Getting shared directories..."); scan_shared_directories(); shell_status_update("Reading sensors..."); read_sensors(); shell_status_update("Obtaining network information..."); scan_net_interfaces(); computer->date_time = "..."; return computer; }
// begin code void setup() { // initialize the serial communication with the server Serial.begin(9600); // start the wire protocol Wire.begin(); // initialize communication with the sabertooth motor controller sabertoothSerial.begin(19200); sabertoothSerial.write(uint8_t(0)); // initialize the interrupts on encoders pinMode(LeftEncoderPin, INPUT); // set pin to input digitalWrite(LeftEncoderPin, HIGH); // turn on pullup resistors attachInterrupt(LeftEncoderPin - 2, left_encoder_interrupt, FALLING); pinMode(RightEncoderPin, INPUT); // set pin to input digitalWrite(RightEncoderPin, HIGH); // turn on pullup resistors attachInterrupt(RightEncoderPin - 2, right_encoder_interrupt, FALLING); // initialize the led pin pinMode(StoppedLEDPin, OUTPUT); pinMode(WarnLEDPin, OUTPUT); pinMode(RunLEDPin, OUTPUT); // turn on the warn led to indicate calibration digitalWrite(WarnLEDPin, HIGH); int h_reads = 0, v_reads = 0, reads = 16; for (int i =0; i < reads; i++) { read_sensors(); h_reads += horizontal.value(); v_reads += vertical.value(); } drive.h_center = h_reads / reads; drive.v_center = v_reads / reads; // calibration complete digitalWrite(WarnLEDPin, LOW); }
void * production_thread(void * dummy) { SystemState data; while (!done) { // Fill the data structure memset(&data,0,sizeof(SystemState)); rtx_thread_setcancel(RTX_THREAD_CANCEL_DISABLE); read_uptime(&data); read_mem(&data); if (readsensors) { read_sensors(&data); } rtx_thread_setcancel(RTX_THREAD_CANCEL_DEFERRED); if (done) break; // Write the data to the store dataV.t_writefrom(data); rtx_timer_sleep(period); } return NULL; }
/** Go forward by a certain specified number of steps. */ void go_distance(unsigned char x) { reset_shaft_counters(); forward(); velocity(100,100); PORTJ = 0x00; while(1){ read_sensors(); print_sensor_data(); if( Front_IR_Sensor<0xF0) { stop(); buzzer_on(); } else { forward(); buzzer_off(); } if((ShaftCountLeft + ShaftCountRight)*5 > x*10) break; } velocity(0,0); }
int main() { ADCinit(); sei(); read_sensors(); TWIslave_init(); //------------------------------- // Lite Test //------------------------------- DDRB = 0xFF; direction = 0; for(;;) { look_for_walls(); PORTB = get_local_wall(1,1); } return 0; }
int main(void) { CanopyContext ctx; CanopyResultEnum result; uint64_t reportTimer = 0; result = canopy_set_global_opt( CANOPY_LOG_LEVEL, 0, CANOPY_LOG_PAYLOADS, true); ctx = canopy_init_context(); if (!ctx) { fprintf(stderr, "Error initializing context\n"); return -1; } result = canopy_set_opt(ctx, CANOPY_DEVICE_UUID, "e6968460-f010-48ef-8e69-835543843b32", CANOPY_DEVICE_SECRET_KEY, "/pMdwTzEA3+d66qo3MZQ2bWjsYGXAHAb", CANOPY_CLOUD_SERVER, "sandbox.canopy.link" ); if (result != CANOPY_SUCCESS){ fprintf(stderr, "Failed to configure context\n"); return -1; } result = canopy_var_init(ctx, "out float32 temperature"); if (result != CANOPY_SUCCESS){ fprintf(stderr, "Failed to init cloudvar 'temperature'\n"); return -1; } result = canopy_var_init(ctx, "out float32 humidity"); if (result != CANOPY_SUCCESS){ fprintf(stderr, "Failed to init cloudvar 'humidity'\n"); return -1; } result = canopy_var_init(ctx, "inout int8 fan_speed"); if (result != CANOPY_SUCCESS){ fprintf(stderr, "Failed to init cloudvar 'fan_speed'\n"); return -1; } /*result = canopy_var_on_change(ctx, "fan_speed", on_fan_speed_change, NULL); if (result != CANOPY_SUCCESS) { fprintf(stderr, "Error setting up fan_speed callback\n"); return -1; }*/ // turn fan off init_fan_pins(); result = canopy_var_set_int8(ctx, "fan_speed", 0); if (!ctx) { fprintf(stderr, "Error setting fan_speed\n"); return -1; } while (1) { int8_t speed; canopy_sync_blocking(ctx, 10*CANOPY_SECONDS); result = canopy_var_get_int8(ctx, "fan_speed", &speed); if (result != CANOPY_SUCCESS) { fprintf(stderr, "Error reading fan_speed\n"); return -1; } set_fan_speed(speed); if (canopy_once_every(&reportTimer, 60*CANOPY_SECONDS)) { printf("reading sensors...\n"); float temperature, humidity; bool sensorOk; sensorOk = read_sensors(&temperature, &humidity); if (sensorOk) { printf("Temperature: %f Humidity: %f\n", temperature, humidity); result = canopy_var_set_float32(ctx, "temperature", temperature); if (result != CANOPY_SUCCESS) { fprintf(stderr, "Failed to set cloudvar 'temperature'\n"); return -1; } result = canopy_var_set_float32(ctx, "humidity", humidity); if (result != CANOPY_SUCCESS) { fprintf(stderr, "Failed to set cloudvar 'humidity'\n"); return -1; } } } } }
void Replay::loop() { while (true) { char type[5]; if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) { if (!hal.util->get_soft_armed()) { hal.util->set_soft_armed(true); ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis()); } } if (!logreader.update(type)) { ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f); fclose(plotf); break; } read_sensors(type); if (streq(type,"ATT")) { Vector3f ekf_euler; Vector3f velNED; Vector3f posNED; Vector3f gyroBias; float accelWeighting; float accelZBias1; float accelZBias2; Vector3f windVel; Vector3f magNED; Vector3f magXYZ; Vector3f DCM_attitude; Vector3f ekf_relpos; Vector3f velInnov; Vector3f posInnov; Vector3f magInnov; float tasInnov; float velVar; float posVar; float hgtVar; Vector3f magVar; float tasVar; Vector2f offset; uint16_t faultStatus; const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_rotation_body_to_ned(); dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z); _vehicle.EKF.getEulerAngles(ekf_euler); _vehicle.EKF.getVelNED(velNED); _vehicle.EKF.getPosNED(posNED); _vehicle.EKF.getGyroBias(gyroBias); _vehicle.EKF.getIMU1Weighting(accelWeighting); _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2); _vehicle.EKF.getWind(windVel); _vehicle.EKF.getMagNED(magNED); _vehicle.EKF.getMagXYZ(magXYZ); _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov); _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); _vehicle.EKF.getFilterFaults(faultStatus); _vehicle.EKF.getPosNED(ekf_relpos); Vector3f inav_pos = _vehicle.inertial_nav.get_position() * 0.01f; float temp = degrees(ekf_euler.z); if (temp < 0.0f) temp = temp + 360.0f; fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", AP_HAL::millis() * 0.001f, logreader.get_sim_attitude().x, logreader.get_sim_attitude().y, logreader.get_sim_attitude().z, _vehicle.barometer.get_altitude(), logreader.get_attitude().x, logreader.get_attitude().y, wrap_180_cd(logreader.get_attitude().z*100)*0.01f, logreader.get_inavpos().x, logreader.get_inavpos().y, logreader.get_ahr2_attitude().x, logreader.get_ahr2_attitude().y, wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f, degrees(DCM_attitude.x), degrees(DCM_attitude.y), degrees(DCM_attitude.z), degrees(ekf_euler.x), degrees(ekf_euler.y), degrees(ekf_euler.z), inav_pos.x, inav_pos.y, inav_pos.z, ekf_relpos.x, ekf_relpos.y, -ekf_relpos.z); fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", AP_HAL::millis() * 0.001f, degrees(ekf_euler.x), degrees(ekf_euler.y), temp, velNED.x, velNED.y, velNED.z, posNED.x, posNED.y, posNED.z, 60*degrees(gyroBias.x), 60*degrees(gyroBias.y), 60*degrees(gyroBias.z), windVel.x, windVel.y, magNED.x, magNED.y, magNED.z, magXYZ.x, magXYZ.y, magXYZ.z, logreader.get_attitude().x, logreader.get_attitude().y, logreader.get_attitude().z); // define messages for EKF1 data packet int16_t roll = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg) int16_t pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg) uint16_t yaw = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg) float velN = (float)(velNED.x); // velocity North (m/s) float velE = (float)(velNED.y); // velocity East (m/s) float velD = (float)(velNED.z); // velocity Down (m/s) float posN = (float)(posNED.x); // metres North float posE = (float)(posNED.y); // metres East float posD = (float)(posNED.z); // metres Down float gyrX = (float)(6000*degrees(gyroBias.x)); // centi-deg/min float gyrY = (float)(6000*degrees(gyroBias.y)); // centi-deg/min float gyrZ = (float)(6000*degrees(gyroBias.z)); // centi-deg/min // print EKF1 data packet fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), roll, pitch, yaw, velN, velE, velD, posN, posE, posD, gyrX, gyrY, gyrZ); // define messages for EKF2 data packet int8_t accWeight = (int8_t)(100*accelWeighting); int8_t acc1 = (int8_t)(100*accelZBias1); int8_t acc2 = (int8_t)(100*accelZBias2); int16_t windN = (int16_t)(100*windVel.x); int16_t windE = (int16_t)(100*windVel.y); int16_t magN = (int16_t)(magNED.x); int16_t magE = (int16_t)(magNED.y); int16_t magD = (int16_t)(magNED.z); int16_t magX = (int16_t)(magXYZ.x); int16_t magY = (int16_t)(magXYZ.y); int16_t magZ = (int16_t)(magXYZ.z); // print EKF2 data packet fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), accWeight, acc1, acc2, windN, windE, magN, magE, magD, magX, magY, magZ); // define messages for EKF3 data packet int16_t innovVN = (int16_t)(100*velInnov.x); int16_t innovVE = (int16_t)(100*velInnov.y); int16_t innovVD = (int16_t)(100*velInnov.z); int16_t innovPN = (int16_t)(100*posInnov.x); int16_t innovPE = (int16_t)(100*posInnov.y); int16_t innovPD = (int16_t)(100*posInnov.z); int16_t innovMX = (int16_t)(magInnov.x); int16_t innovMY = (int16_t)(magInnov.y); int16_t innovMZ = (int16_t)(magInnov.z); int16_t innovVT = (int16_t)(100*tasInnov); // print EKF3 data packet fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), innovVN, innovVE, innovVD, innovPN, innovPE, innovPD, innovMX, innovMY, innovMZ, innovVT); // define messages for EKF4 data packet int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX)); int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX)); int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX)); int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); // print EKF4 data packet fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, (unsigned)AP_HAL::millis(), (int)sqrtvarV, (int)sqrtvarP, (int)sqrtvarH, (int)sqrtvarMX, (int)sqrtvarMY, (int)sqrtvarMZ, (int)sqrtvarVT, (int)offsetNorth, (int)offsetEast, (int)faultStatus); } } flush_dataflash(); if (check_solution) { report_checks(); } exit(0); }
void main(void) { device_init(); //START START_MOTOR; FORWARD; set_speed(SPEED_MIN); while (1) { //BREAKERS if(LEFT_BREAKER) { //----------------------- BACKWARD; set_speed(SPEED_BACKWARD);delay_ms(50); set_servo(MAX_LEFT);delay_ms(150); STOP; set_servo(MAX_RIGHT);delay_ms(100); FORWARD; set_speed(SPEED_MIN);delay_ms(20); continue; //----------------------- }; if(RIGHT_BREAKER) { //----------------------- BACKWARD; set_speed(SPEED_BACKWARD);delay_ms(50); set_servo(MAX_RIGHT);delay_ms(150); STOP; set_servo(MAX_LEFT);delay_ms(100); FORWARD; set_speed(SPEED_MIN);delay_ms(20); continue; //----------------------- }; //IR_SENSOR if(IR_SENSOR) { BACKWARD; set_speed(SPEED_BACKWARD);delay_ms(30); set_servo(MAX_LEFT);delay_ms(70); STOP; set_servo(MAX_RIGHT);delay_ms(100); FORWARD; set_speed(SPEED_MIN);delay_ms(20); continue; }; //reading Sharp2D120X read_sensors(); //////////////////////////////////////////////// //--------------------------------------------- //bot control bot_control(); /////////////////////////////////////////////// //LCD to_lcd(); }; }
void Replay::setup() { ::printf("Starting\n"); uint8_t argc; char * const *argv; hal.util->commandline_arguments(argc, argv); _parse_command_line(argc, argv); // _parse_command_line sets up an FPE handler. We can do better: signal(SIGFPE, _replay_sig_fpe); hal.console->printf("Processing log %s\n", filename); if (update_rate == 0) { update_rate = find_update_rate(filename); } hal.console->printf("Using an update rate of %u Hz\n", update_rate); if (!logreader.open_log(filename)) { perror(filename); exit(1); } _vehicle.setup(); set_ins_update_rate(update_rate); logreader.wait_type("GPS"); logreader.wait_type("IMU"); logreader.wait_type("GPS"); logreader.wait_type("IMU"); feenableexcept(FE_INVALID | FE_OVERFLOW); plotf = fopen("plot.dat", "w"); plotf2 = fopen("plot2.dat", "w"); ekf1f = fopen("EKF1.dat", "w"); ekf2f = fopen("EKF2.dat", "w"); ekf3f = fopen("EKF3.dat", "w"); ekf4f = fopen("EKF4.dat", "w"); fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n"); fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n"); fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n"); fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n"); fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n"); ::printf("Waiting for GPS\n"); while (!done_home_init) { char type[5]; if (!logreader.update(type)) { break; } read_sensors(type); if (streq(type, "GPS") && (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init && !done_home_init) { const Location &loc = _vehicle.gps.location(); ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", loc.lat * 1.0e-7f, loc.lng * 1.0e-7f, loc.alt * 0.01f, hal.scheduler->millis()*0.001f); _vehicle.ahrs.set_home(loc); _vehicle.compass.set_initial_location(loc.lat, loc.lng); done_home_init = true; } } }
void adjust() { read_sensors(); calc_fan(); set_fan(); }
//This is a non-blocking function. It is called once on every iteration of the main loop if "white_line_flag" is ON //It reads the whiteline sensor values and determines what it should do next to stay on the white line void whiteline_follow_continue() { read_sensors(); flag=0; print_sensor_data(); if(Center_white_line<W_THRESHOLD_STOP && Left_white_line<W_THRESHOLD_STOP && Right_white_line<W_THRESHOLD_STOP ){ if (whiteline_stop_intersection_flag) { whiteline_follow_end(); send_char(SUCCESS); } /*else { forward(); velocity(100,100); stop_on_timer4_overflow = 1; start_timer4(); while (stop_on_timer4_overflow != 0) {;} } return;*/ } if( Front_IR_Sensor<0xF0) { stop(); buzzer_on(); } //Sensor config : 010 else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD) { forward(); velocity(150,150); black_flag = 0; buzzer_off(); } //Sensor config : 110 else if(Left_white_line < W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD) { forward(); velocity(120,150); black_flag = 0; buzzer_off(); } //Sensor config : 100 else if(Left_white_line < W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line > W_THRESHOLD) { PORTA = 0x05; velocity(50,130); last_on = LEFT_SENSOR; black_flag = 0; buzzer_off(); } //Sensor config : 011 else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line < W_THRESHOLD) { forward(); velocity(150,120); black_flag = 0; buzzer_off(); } //Sensor config : 001 else if(Left_white_line > W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line < W_THRESHOLD) { PORTA = 0x0A; velocity(130,50); last_on = RIGHT_SENSOR; black_flag = 0; buzzer_off(); } //Sensor config : 000 else { buzzer_off(); if(black_flag >= CONT_BLACK) { if(last_on == LEFT_SENSOR) motion_set(0x05); else if(last_on == RIGHT_SENSOR) motion_set(0x0A); velocity(100,100); while(1){ print_sensor_data(); read_sensors(); if(Center_white_line < W_THRESHOLD) break; } } black_flag = (black_flag < CONT_BLACK)?black_flag+1:CONT_BLACK; forward(); velocity(0,0); } }
int main() { int i; HTS221 hts221; pc.baud(115200); void hts221_init(void); // Set LED to RED until init finishes SetLedColor(0x1); pc.printf(BLU "Hello World from AT&T Shape!\r\n\n\r"); pc.printf(GRN "Initialize the HTS221\n\r"); i = hts221.begin(); if( i ) pc.printf(BLU "HTS221 Detected! (0x%02X)\n\r",i); else pc.printf(RED "HTS221 NOT DETECTED!!\n\r"); printf("Temp is: %0.2f F \n\r",CTOF(hts221.readTemperature())); printf("Humid is: %02d %%\n\r",hts221.readHumidity()); sensors_init(); read_sensors(); // Initialize the modem printf(GRN "Modem initializing... will take up to 60 seconds" DEF "\r\n"); do { i=mdm_init(); if (!i) { pc.printf(RED "Modem initialization failed!" DEF "\n"); } } while (!i); //Software init software_init_mdm(); // Resolve URL to IP address to connect to resolve_mdm(); //Create a 1ms timer tick function: OneMsTicker.attach(OneMsFunction, 0.001f) ; iTimer1Interval_ms = SENSOR_UPDATE_INTERVAL_MS; // Open the socket (connect to the server) sockopen_mdm(); // Set LED BLUE for partial init SetLedColor(0x4); // Send and receive data perpetually while(1) { static unsigned ledOnce = 0; if (bTimerExpiredFlag) { bTimerExpiredFlag = false; sprintf(SENSOR_DATA.Temperature, "%0.2f", CTOF(hts221.readTemperature())); sprintf(SENSOR_DATA.Humidity, "%02d", hts221.readHumidity()); read_sensors(); //read available external sensors from a PMOD and the on-board motion sensor char modem_string[512]; GenerateModemString(&modem_string[0]); printf(BLU "Sending to modem : %s" DEF "\n", modem_string); sockwrite_mdm(modem_string); sockread_mdm(&MySocketData, 1024, 20); // If any non-zero response from server, make it GREEN one-time // then the actual FLOW responses will set the color. if ((!ledOnce) && (MySocketData.length() > 0)) { ledOnce = 1; SetLedColor(0x2); } printf(BLU "Read back : %s" DEF "\n", &MySocketData[0]); char * myJsonResponse; if (extract_JSON(&MySocketData[0], &myJsonResponse[0])) { printf(GRN "JSON : %s" DEF "\n", &myJsonResponse[0]); parse_JSON(&myJsonResponse[0]); } else { printf(RED "JSON : %s" DEF "\n", &myJsonResponse[0]); //most likely an incomplete JSON string parse_JSON(&myJsonResponse[0]); //This is risky, as the string may be corrupted } } //bTimerExpiredFlag } //forever loop }
int main(int argc, char**argv) { ros::init(argc,argv,"imu"); ros::NodeHandle n; ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000); ros::NodeHandle nh; ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000); sensor_msgs::Imu imu_msg; std_msgs::Float64 msg; // Read sensors, init DCM algorithm reset_sensor_fusion(); removegyrooff(); float Y=0.0; int i=0; while(ros::ok()) { // Time to read the sensors again? if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL) { timestamp_old = timestamp; timestamp = clock(); if (timestamp > timestamp_old) G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; //cout << G_Dt << endl; // Update sensor readings read_sensors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); output_angles(); imu_msg = sensor_msgs::Imu(); imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = "imu"; imu_msg.orientation.x = TO_DEG(pitch); imu_msg.orientation.y = TO_DEG(roll); imu_msg.orientation.z = TO_DEG(yaw); imu_msg.orientation.w = 0.0; imu_msg.orientation_covariance[0] = -1; imu_msg.angular_velocity.x = 0.0; imu_msg.angular_velocity.y = 0.0; imu_msg.angular_velocity.z = 0.0; imu_msg.angular_velocity_covariance[0] = -1; imu_msg.linear_acceleration.x = 0.0; imu_msg.linear_acceleration.y = 0.0; imu_msg.linear_acceleration.z = 0.0; imu_msg.linear_acceleration_covariance[0] = -1; msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0])); chatter_pub.publish(msg); ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0]))); //ros::spinOnce(); } } }
// Main loop void razor_loop(void) { razor_loop_input(); // Time to read the sensors again? if ((timer_systime() - timestamp) >= OUTPUT__DATA_INTERVAL) { timestamp_old = timestamp; timestamp = timer_systime(); if (timestamp > timestamp_old) G_Dt = (double)MS2S(timestamp - timestamp_old); // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0.0; // Update sensor readings read_sensors(); switch (output_mode) { case OUTPUT__MODE_CALIBRATE_SENSORS: // We're in calibration mode { check_reset_calibration_session(); // Check if this session needs a reset if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor); break; } case OUTPUT__MODE_YPR_RAZOR: { // Apply sensor calibration compensate_sensor_errors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); if (output_stream_on || output_single_on) output_angles(); break; } case OUTPUT__MODE_YPR_MADGWICK: { compensate_sensor_errors(); gyro[0] = GYRO_SCALED_RAD(gyro[0]); gyro[1] = GYRO_SCALED_RAD(gyro[1]); gyro[2] = GYRO_SCALED_RAD(gyro[2]); // acc and mag already scaled in compensate_sensor_errors() // Apply Madgwick algorithm MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]); //MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]); //MadgwickAHRSupdate(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, magnetom[0], magnetom[1], magnetom[2]); MadgwickYawPitchRoll(&yaw, &pitch, &roll); if (output_stream_on || output_single_on) output_angles(); break; } case OUTPUT__MODE_YPR_FREEIMU: { compensate_sensor_errors(); gyro[0] = GYRO_SCALED_RAD(gyro[0]); gyro[1] = GYRO_SCALED_RAD(gyro[1]); gyro[2] = GYRO_SCALED_RAD(gyro[2]); // acc and mag already scaled in compensate_sensor_errors() // get sensorManager and initialise sensor listeners //mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE); //initListeners(); // wait for one second until gyroscope and magnetometer/accelerometer // data is initialised then scedule the complementary filter task //fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT); //public void onSensorChanged(SensorEvent event) { //switch(event.sensor.getType()) { //case Sensor.TYPE_ACCELEROMETER: // copy new accelerometer data into accel array and calculate orientation //System.arraycopy(event.values, 0, accel, 0, 3); calculateAccMagOrientation(); //case Sensor.TYPE_GYROSCOPE: // process gyro data //gyroFunction(event); gyroFunction(); //case Sensor.TYPE_MAGNETIC_FIELD: // copy new magnetometer data into magnet array //System.arraycopy(event.values, 0, magnet, 0, 3); calculateFusedOrientation(); yaw = -fusedOrientation[0]; roll = -fusedOrientation[1]; pitch = fusedOrientation[2]; //if (output_stream_on || output_single_on) output_angles_freedom(); if (output_stream_on || output_single_on) output_angles(); break; } case OUTPUT__MODE_SENSORS_RAW: case OUTPUT__MODE_SENSORS_CALIB: case OUTPUT__MODE_SENSORS_BOTH: { if (output_stream_on || output_single_on) output_sensors(); break; } } output_single_on = false; #if DEBUG__PRINT_LOOP_TIME == true //printf("loop time (ms) = %lu\r\n", timer_systime() - timestamp); // Not really useful since RTC measures only 4 ms. #endif } #if DEBUG__PRINT_LOOP_TIME == true else { printf("waiting...\r\n"); } #endif return; }