void params_of_att(double *a, double *b, int *c, const quat_t *att) { const xyz_t e_x={1,0,0}; xyz_t v; rot_vec_by_quat_a2b(&v, att, &e_x); *a = v.y; *b = v.z; *c = (v.x > 0); }
void toytronics_set_sp_incremental_from_rc() { double dt = 1.0/RC_UPDATE_FREQ; const rc_t * const rc = get_rc(); const quat_t * const q_n2b = get_q_n2b(); // local copies to allow implementing a deadband double rcp = apply_deadband(rc->pitch, SETPOINT_DEADBAND); double rcr = apply_deadband(rc->roll, SETPOINT_DEADBAND); double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND); // rotation vector in body frame xyz_t w_dt_body = {rcr * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt, rcp * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt, rcy * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt}; // try accelerometer turn coordination w_dt_body.z -= dt*tc_fading(q_n2b)*aerobatic_accel_tc_gain*get_y_accel(); // old body to setpoint quat q_b2sp quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp)); // rotation vector in setpoint frame xyz_t w_dt_sp; rot_vec_by_quat_a2b( &w_dt_sp, &(setpoint.q_b2sp), &w_dt_body); // form diff quat double total_angle = sqrt( w_dt_sp.x*w_dt_sp.x + w_dt_sp.y*w_dt_sp.y + w_dt_sp.z*w_dt_sp.z + 1e-9); quat_t diff_quat = {cos(total_angle/2.0), sin(total_angle/2.0)*w_dt_sp.x/total_angle, sin(total_angle/2.0)*w_dt_sp.y/total_angle, sin(total_angle/2.0)*w_dt_sp.z/total_angle}; // use diff quat to update setpoint quat setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, diff_quat); // calculate body to setpoint quat quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp)); // now bound setpoint quat to not get too far away from estimated quat BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0); BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0); BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0); // let setpoint decay back to body discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt ); discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt ); discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt ); // normalize setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3)); // update n2sp quat quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp)); // set stabilization setpoint set_stabilization_setpoint(&setpoint.q_n2sp); }
int project_o_tron(aero_table_output_t * out, aero_model_t * model, quat_t ref_to_body, int w, int h, int *pix_buf_panel) { int n = model->n_panels; // *** Input parameters xyz_t UU e_v_ref = { 1, 0, 0 }; // velocity unit vector in ref frame // todo: check that is normalized double res = 0.001 / MEGARES; // Resolution double sig_n = 0.8; // normal momentum exchange coeff double sig_t = 0.8; // tangential momentum exchange coeff // *** Let's choose a new "freestream" frame so that the velocity vector is just +X quat_t body_to_fs; xyz_t e_v_fs = { 1, 0, 0 }; // Don't need to rotate at all, the frames are already aligned // Now form the quaternion rotating the body frame to the freestream frame quat_inv(&body_to_fs, &ref_to_body); // *** Rotate "everything" into the new frame // // Rotate the center of mass xyz_t cg_fs; rot_vec_by_quat_a2b(&cg_fs, &body_to_fs, &model->cg); // Rotate the panels' coordinates panel_t panels_fs[model->n_panels]; for (int i = 0; i < n; i++) { rot_vec_by_quat_a2b(&panels_fs[i].origin, &body_to_fs, &model->panels[i].origin); rot_vec_by_quat_a2b(&panels_fs[i].side_a, &body_to_fs, &model->panels[i].side_a); rot_vec_by_quat_a2b(&panels_fs[i].side_b, &body_to_fs, &model->panels[i].side_b); } // Find the centroids xyz_t centroid_fs[n]; for (int i = 0; i < n; i++) { centroid_fs[i] = panels_fs[i].origin; // This handy function does (a <= a*A + b*B + c*C) xyz_sum_scale(¢roid_fs[i], 1, &panels_fs[i].side_a, 0.5, &panels_fs[i].side_b, 0.5); } // Sort by the X coordinate of the centroids struct indirect_element sort_list[n]; for (int i = 0; i < n; i++) { sort_list[i].index = i; sort_list[i].val = centroid_fs[i].x; } qsort(sort_list, n, sizeof(sort_list[0]), compare_indirect_elements); double *pix_buf_x = malloc(w * h * sizeof(double)); // Holds the fs frame X coordinate // of the panel element at each pixel if (!pix_buf_x) { fprintf(stderr, "Unable to allocate %zu bytes for pix_buf_x\n", w * h * sizeof(double)); return 1; } // Iterate over the panels, rearmost centroid first for (int i = 0; i < n; i++) { // if(sort_list[i].index==0){ panel_t *p = &panels_fs[sort_list[i].index]; //printf("Panel %d:::\n",sort_list[i].index); double res_a = res / xyz_norm(&p->side_a) / sqrt(2); double res_b = res / xyz_norm(&p->side_b) / sqrt(2); // Iterate over side a for (double a = 0; a <= 1; a += res_a) { // Iterate over side b for (double b = 0; b <= 1; b += res_b) { xyz_t panel_cell = p->origin; xyz_sum_scale(&panel_cell, 1, &p->side_a, a, &p->side_b, b); int pix_y = (int)round(panel_cell.y / res + w / 2); int pix_z = (int)round(panel_cell.z / res + h / 2); pix_buf_panel[pix_z + pix_y * w] = sort_list[i].index + 1; pix_buf_x[pix_z + pix_y * w] = panel_cell.x; // printf("\t%+7.4f\t%+7.4f\t%d\t%d\n",panel_cell.y,panel_cell.z,pix_y,pix_z); } } } // Find the unit normal vector in fs frame, and e_v.e_n for all panels xyz_t e_n_fs[n]; double e_v_dot_e_n[n]; for (int p = 0; p < n; p++) { xyz_cross(&e_n_fs[p], &panels_fs[p].side_a, &panels_fs[p].side_b); xyz_normalize_self(&e_n_fs[p]); e_v_dot_e_n[p] = xyz_dot(&e_n_fs[p], &e_v_fs); // if (e_v_dot_e_n[p] < 0) // printf("e_v dot e_n negative for panel %d\n",p); } // double A = 0; int n_pix[n]; memset(n_pix, 0, sizeof(n_pix)); xyz_t panel_sum_pos[n]; memset(panel_sum_pos, 0, sizeof(panel_sum_pos)); //printf("sizeof(n_pix)=%zu\n",sizeof(n_pix)); xyz_t F_fs = {0, 0, 0}; xyz_t T_fs = {0, 0, 0}; // Now iterate over the pixels for (int pix_y = 0; pix_y < w; pix_y++) for (int pix_z = 0; pix_z < h; pix_z++) { int p = pix_buf_panel[pix_z + pix_y * w]; if (p) { // There is a panel at this pixel. p--; // Actual panel index number is one less if (e_v_dot_e_n[p] < 0) { // Skip panels facing the wrong way continue; } n_pix[p]++; // Find the coordinates in the fs frame xyz_t fs_p; fs_p.x = pix_buf_x[pix_z * w + pix_y]; fs_p.y = (pix_y - w / 2) * res; fs_p.z = (pix_z - h / 2) * res; // Where is it wrt the center of mass? xyz_t r_from_cg_fs; xyz_diff(&r_from_cg_fs, &fs_p, &cg_fs); // Let's compute the forces on this element // ref: NASA SP-8058 eq 2-2 (page 5). // Dynamic pressure (1/2 rho v^2) factored out, hence leading 2 * xyz_t dF = { 0, 0, 0 }; xyz_sum_scale(&dF, 0, &e_n_fs[p], 2 * -1 * res * res * (2 - sig_n - sig_t) * xyz_dot(&e_v_fs, &e_n_fs[p]), &e_v_fs, 2 * -1 * res * res * sig_t); // Torque contribution from this element xyz_t dT; xyz_cross(&dT, &r_from_cg_fs, &dF); xyz_sum(&F_fs, &F_fs, &dF); xyz_sum(&T_fs, &T_fs, &dT); } } rot_vec_by_quat_b2a(&out->force, &body_to_fs, &F_fs); rot_vec_by_quat_b2a(&out->torque, &body_to_fs, &T_fs); printf("F = %f %f %f (norm=%f)\n",out->force.x, out->force.y, out->force.z, xyz_norm(&out->force)); printf("T = %f %f %f (norm=%f)\n",out->torque.x, out->torque.y, out->torque.z, xyz_norm(&out->torque)); // printf("%d pixels dropped due to e_v dot e_n negative\n",n_pix); free(pix_buf_x); return 0; }