void SensorState::ahrs_log_state(uint64_t now)
{
logger(LOG_INFO, "SensorState twoKp=%f twoKi=%f integralFBx=%f integralFBy=%f integralFBz=%f",
m_state.twoKp,
m_state.twoKi,
m_state.integralFBx,
m_state.integralFBy,
m_state.integralFBz
);
logger(LOG_INFO, "SensorState orientation=%f:%f:%f:%f",
m_state.orient.q0,
m_state.orient.q1,
m_state.orient.q2,
m_state.orient.q3
);
// For logging purposes, let's spit out Euler angles:
const ahrs::Quaternion &q = m_state.orient;
const float x = atan2(2 * (q.q0 + q.q1), 1 - 2 * (q.q1 * q.q1 + q.q2 * q.q2));
const float y = asin(2 * (q.q0 * q.q2 - q.q3 * q.q1));
const float z = atan2(2 * (q.q0 * q.q3 + q.q1 * q.q2), 1 - 2 * (q.q2 * q.q2 + q.q3 * q.q3));
logger(LOG_INFO, "SensorState Euler=%f:%f:%f", to_degree(x), to_degree(y), to_degree(z));
}
// ### Experimental racing function
double race_to(double curr_coord[2], double x, double y){
double steering = 1.8; // Ratio between the speed of each wheels.
double error_margin_angle = 3; // Degrees
double speed = 70;
double dx = x - curr_coord[0];
double dy = y - curr_coord[1];
double targetA = atan2(dx, dy);
double dangle = targetA - face_angle;
dangle += (dangle > to_rad(180)) ? -to_rad(360) : (dangle < -to_rad(180)) ? to_rad(360) : 0;
double distance = fabs(sqrt(dx*dx + dy*dy));
// If we need to turn, perform steering maneuver
if(fabs(dangle) >= to_rad(error_margin_angle)){
if (to_degree(dangle) > 0){
set_motors( speed*steering, speed/steering);
}
else {
set_motors(speed/steering, speed*steering);
}
printf("Steering: %f \n", to_degree(dangle));
}
else { // Otherwise just go straight;
set_motors(speed, speed);
}
position_tracker(curr_coord);
return distance;
}
static unsigned long horner_par
(unsigned long x, const unsigned long* a, unsigned long n)
{
/* stealcontext flags */
static const unsigned long sc_flags =
KAAPI_SC_CONCURRENT | KAAPI_SC_PREEMPTION;
/* self thread, task */
kaapi_thread_t* const thread = kaapi_self_thread();
kaapi_taskadaptive_result_t* ktr;
kaapi_stealcontext_t* sc;
/* sequential indices */
unsigned long i, j;
horner_work_t work;
/* initialize horner work */
work.x = x;
work.a = a;
work.n = n;
work.res = a[to_index(n, n)];
kaapi_workqueue_init(&work.wq, 0, n);
/* enter adaptive section */
sc = kaapi_task_begin_adaptive(thread, sc_flags, splitter, &work);
continue_work:
while (extract_seq(&work, &i, &j) != -1)
{
const unsigned long hi = to_degree(i, n);
const unsigned long lo = to_degree(j, n);
work.res = horner_seq_hilo(x, a, n, work.res, hi, lo);
}
/* preempt and reduce thieves */
if ((ktr = kaapi_get_thief_head(sc)) != NULL)
{
kaapi_preempt_thief
(sc, ktr, (void*)&work, victim_reducer, (void*)&work);
goto continue_work;
}
/* wait for thieves */
kaapi_task_end_adaptive(sc);
return work.res;
}
static void thief_entrypoint
(void* args, kaapi_thread_t* thread, kaapi_stealcontext_t* sc)
{
/* input work */
horner_work_t* const work = (horner_work_t*)args;
/* resulting work */
horner_res_t* const res = kaapi_adaptive_result_data(sc);
unsigned long i, j;
unsigned int is_preempted;
/* set the splitter for this task */
kaapi_steal_setsplitter(sc, splitter, work);
while (extract_seq(work, &i, &j) != -1)
{
const unsigned long hi = to_degree(i, work->n);
const unsigned long lo = to_degree(j, work->n);
res->res = horner_seq_hilo
(work->x, work->a, work->n, res->res, hi, lo);
kaapi_steal_setsplitter(sc, NULL, NULL);
kaapi_synchronize_steal(sc);
res->i = (unsigned long)work->wq.beg;
res->j = (unsigned long)work->wq.end;
is_preempted = kaapi_preemptpoint
(sc, thief_reducer, NULL, NULL, 0, NULL);
if (is_preempted) return ;
kaapi_steal_setsplitter(sc, splitter, work);
}
/* update our results. use beg == beg
to avoid the need of synchronization
with potential victim .end update
*/
res->i = work->wq.beg;
res->j = work->wq.beg;
}
static void thief_entrypoint
(void* args, kaapi_thread_t* thread, kaapi_stealcontext_t* sc)
{
/* input work */
thief_work_t* const work = (thief_work_t*)args;
/* resulting work */
thief_work_t* const res_work = kaapi_adaptive_result_data(sc);
const unsigned long hi = to_degree(work->i, work->n);
const unsigned long lo = to_degree(work->j, work->n);
work->res = horner_seq_hilo
(work->x, work->a, work->n, work->res, hi, lo);
/* update work indices */
work->i = work->j;
/* we are finished, update results. */
memcpy(res_work, work, sizeof(thief_work_t));
}
int node_on_left(double angle, struct node* currentnode){
angle = to_degree(angle);
if (angle < 30 && angle > -30)
return currentnode->name - 1;
else if (angle < 120 && angle > 60)
return currentnode->name + 4;
else if (angle > -120 && angle < -60)
return currentnode->name - 4;
else if (angle < -150 || angle > 150)
return currentnode->name + 1;
else return 17;
}
void spin(double curr_coord[2], double angle){
if (angle == 0)
return;
printf("Start Spinning: from %f, with :%f\n",to_degree(face_angle), to_degree(angle));
double initial_angle = face_angle;
int speed = 15;
int tempspeed = 0;
double angle_turned = 0;
double abs_angle = fabs(angle);
double side = angle/fabs(angle);
int tempenc[2] = {0, 0};
while (angle_turned < abs_angle){
get_motor_encoders(&tempenc[0], &tempenc[1]);
double dl = enc_to_dist(tempenc[0] - prevenc[0]);
double dr = enc_to_dist(tempenc[1] - prevenc[1]);
face_angle += ( dl - dr )/ width;
if (abs_angle < to_rad(30)){
tempspeed = side * 1;
set_motors(tempspeed, -tempspeed);
}
else if(angle_turned >= fabs(0.92*angle)){
tempspeed = (tempspeed < 2) ? side : side * speed * (1 - fabs(angle_turned/angle));
set_motors(tempspeed, -tempspeed);
}
else {
set_motors(side*speed, side*-speed);
}
angle_turned = fabs(face_angle - initial_angle);
prevenc[0] = tempenc[0];
prevenc[1] = tempenc[1];
//printf("Monitoring: angle: %f X: %f , Y: %f \n", to_degree(face_angle), curr_coord[0], curr_coord[1] );
}
position_tracker(curr_coord);
set_motors(0, 0);
printf("(Spinning done ! %f \n", to_degree(face_angle) );
usleep(10000);
}
int node_in_front(double angle, struct node* currentnode){
// Caculate the index of node in the "nodes[]" array, based on it's face_angle
angle = to_degree(angle);
if (currentnode->name == 0)
return 1;
if (angle < 30 && angle > -30)
return currentnode->name + 4;
else if (angle < 120 && angle > 60)
return currentnode->name + 1;
else if (angle > -120 && angle < -60)
return currentnode->name - 1;
else if (angle < -150 || angle > 150)
return currentnode->name - 4;
else return 17; // Basically a "ghost" node as default value, preventing segmentation fault.
}
float Math::polarAngle(sf::Vector2f vec)
{
return to_degree(std::atan2(vec.y, vec.x));
}
/*
* Algorithm from http://williams.best.vwh.net/sunrise_sunset_algorithm.htm
* Inspiration from https://gist.github.com/Tafkas/4742250/
* sunrise == true -> sunrise
* sunrise == false -> sunset
* */
struct tm *calculate_sunrise(bool sunrise, int day, int month, int year, double latitude, double longitude, double local_offset)
{
int day_of_year = calculate_day_of_year(day, month, year);
double zenith = 90.83333333333333;
/* convert the longitude to hour value and calculate an approximate time */
double lngHour = longitude / 15;
double t;
if (sunrise) {
t = day_of_year + (( 6 - lngHour) / 24);
} else {
t = day_of_year + ((18 - lngHour) / 24);
}
/* calculate the Sun's mean anomaly */
double M = (0.9856 * t) - 3.289;
/* calculate the Sun's true longitude */
double L = M + (1.916 * sin(to_radian(M))) + (0.020 * sin(to_radian(2 * M))) + 282.634;
L = adjust_to_range(L, 0, 360);
/* calculate the Sun's right ascension */
double RA = to_degree(atan(0.91764 * tan(to_radian(L))));
RA = adjust_to_range(RA, 0, 360);
/* right ascension value needs to be in the same quadrant as L */
double Lquadrant = (floor( L / 90)) * 90;
double RAquadrant = (floor(RA / 90)) * 90;
RA = RA + (Lquadrant - RAquadrant);
/* right ascension value needs to be converted into hours */
RA = RA / 15;
/* calculate the Sun's declination */
double sinDec = 0.39782 * sin(to_radian(L));
double cosDec = cos(asin(sinDec));
/* calculate the Sun's local hour angle */
double cosH = (cos(to_radian(zenith)) - (sinDec * sin(to_radian(latitude)))) / (cosDec * cos(to_radian(latitude)));
/* no sunrise or no sunset */
if ((cosH > 1) && sunrise) {
return NULL;
} else if ((cosH < -1) && !sunrise) {
return NULL;
}
/* finish calculating H and convert into hours */
double H;
if (sunrise) {
H = 360 - to_degree(acos(cosH));
} else {
H = to_degree(acos(cosH));
}
H = H / 15;
/* calculate local mean time of rising/setting */
double T = H + RA - (0.06571 * t) - 6.622;
/* adjust back to UTC */
double UTC = T - lngHour;
UTC = adjust_to_range(UTC, 0, 24);
/* convert UTC value to local time zone of latitude/longitude */
double localT = UTC + local_offset;
/* localT is in this form: 12.34567
* hour: floor(12.34567) = 12
* minutes: floor(0.34567 * 60) = floor(20.7402) = 20
* seconds: floor(0.7402 * 60) = floor(44.412) = 44
*/
struct tm *time = calloc(1, sizeof(struct tm));
time->tm_mday = day;
time->tm_mon = month - 1; /* January is 0 */
time->tm_year = year - 1900; /* Saved as years from 1900 */
time->tm_hour = floor(localT);
localT -= time->tm_hour;
localT *= 60;
time->tm_min = floor(localT);
localT -= time->tm_min;
localT *= 60;
time->tm_sec = floor(localT);
return time;
}
void move_to(double curr_coord[2], double x, double y){
double dx = x - curr_coord[0];
double dy = y - curr_coord[1];
double targetA = atan2(dx, dy);
double dangle = targetA - face_angle;
dangle += (dangle > to_rad(180)) ? -to_rad(360) : (dangle < -to_rad(180)) ? to_rad(360) : 0;
spin(curr_coord, dangle);
go_straight(curr_coord, fabs(sqrt(dx*dx + dy*dy)));
printf("Moving Done : X = %f, Y = %f, face_angle = %f \n", curr_coord[0], curr_coord[1], to_degree(face_angle));
}
