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;
}