/**
* Set only local XY coordinates of waypoint without update altitude.
* @todo: how to handle global waypoints?
*/
void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_i.x = x;
waypoints[wp_id].enu_i.y = y;
/* also update ENU float representation */
waypoints[wp_id].enu_f.x = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.x);
waypoints[wp_id].enu_f.y = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.y);
waypoint_globalize(wp_id);
}
}
/** Navigation function along a path
*/
static inline bool_t mission_nav_path(struct _mission_element *el)
{
if (el->element.mission_path.nb == 0) {
return FALSE; // nothing to do
}
if (el->element.mission_path.path_idx == 0) { //first wp of path
el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
}
else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
if (nav_approaching_from(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(to_wp, el->duration)) {
el->element.mission_path.path_idx++;
}
} else { el->element.mission_path.path_idx++; }
}
//Route Between from-to
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(from_wp, to_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.);
} else { return FALSE; } //end of path
return TRUE;
}
void ins_update_baro() {
#ifdef USE_VFF
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro.absolute;
ins_baro_initialised = TRUE;
}
ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
if (ins_vf_realign) {
ins_vf_realign = FALSE;
ins_qfe = baro.absolute;
#ifdef BOOZ2_SONAR
ins_sonar_offset = sonar_meas;
#endif
vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
ins_enu_pos.z = -ins_ltp_pos.z;
ins_enu_speed.z = -ins_ltp_speed.z;
ins_enu_accel.z = -ins_ltp_accel.z;
}
vff_update(alt_float);
}
#endif
}
void ins_update_baro() {
#if USE_VFF
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro.absolute;
ins_baro_initialised = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro.absolute;
#if USE_SONAR
ins_sonar_offset = sonar_meas;
#endif
vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { /* not realigning, so normal update with baro measurement */
ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
vff_update(alt_float);
}
}
INS_NED_TO_STATE();
#endif
}
void guidance_indi_run(bool in_flight, int32_t heading) {
//filter accel to get rid of noise
//filter attitude to synchronize with accel
guidance_indi_filter_attitude();
guidance_indi_filter_accel();
float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x; //+-ov.y/ (STABILIZATION_ATTITUDE_SP_MAX_THETA)*3.0;
float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y; //+ ov.x/ (STABILIZATION_ATTITUDE_SP_MAX_PHI)*3.0;
float speed_sp_x = pos_x_err*guidance_indi_pos_gain;
float speed_sp_y = pos_y_err*guidance_indi_pos_gain;
sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x)*guidance_indi_speed_gain;
sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y)*guidance_indi_speed_gain;
// sp_accel.x = (radio_control.values[RADIO_PITCH]/9600.0)*8.0;
// sp_accel.y = -(radio_control.values[RADIO_ROLL]/9600.0)*8.0;
// struct FloatMat33 Ga;
guidance_indi_calcG(&Ga);
MAT33_INV(Ga_inv, Ga);
float altitude_sp = POS_FLOAT_OF_BFP(guidance_v_z_sp);
float vertical_velocity_sp = guidance_indi_pos_gain*(altitude_sp - stateGetPositionNed_f()->z);
// float vertical_velocity_rc_euler = -(stabilization_cmd[COMMAND_THRUST]-4500.0)/4500.0*2.0;
float vertical_velocity_err_euler = vertical_velocity_sp - stateGetSpeedNed_f()->z;
sp_accel.z = vertical_velocity_err_euler*guidance_indi_speed_gain;
struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, 0.0};
Bound(a_diff.x, -6.0, 6.0);
Bound(a_diff.y, -6.0, 6.0);
Bound(a_diff.z, -9.0, 9.0);
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
guidance_euler_cmd.phi = roll_filt + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt + euler_cmd.y;
guidance_euler_cmd.psi = 0;//stateGetNedToBodyEulers_f()->psi;
//Bound euler angles to prevent flipping and keep upright
Bound(guidance_euler_cmd.phi, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
stabilization_attitude_set_setpoint_rp_quat_f(in_flight, heading);
}
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_i.z = alt;
/* also update ENU float representation */
waypoints[wp_id].enu_f.z = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.z);
waypoint_globalize(wp_id);
}
}
void ins_update_sonar() {
static float last_offset = 0.;
// new value filtered with median_filter
ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas);
float sonar = (ins_sonar_alt - ins_sonar_offset) * INS_SONAR_SENS;
#ifdef INS_SONAR_VARIANCE_THRESHOLD
/* compute variance of error between sonar and baro alt */
int32_t err = POS_BFP_OF_REAL(sonar) + ins_baro_alt; // sonar positive up, baro positive down !!!!
var_err[var_idx] = POS_FLOAT_OF_BFP(err);
var_idx = (var_idx + 1) % VAR_ERR_MAX;
float var = variance_float(var_err, VAR_ERR_MAX);
DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var);
//DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_sonar_alt, &sonar, &var);
#endif
/* update filter assuming a flat ground */
if (sonar < INS_SONAR_MAX_RANGE
#ifdef INS_SONAR_MIN_RANGE
&& sonar > INS_SONAR_MIN_RANGE
#endif
#ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD
&& stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
&& ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */
#endif
#ifdef INS_SONAR_VARIANCE_THRESHOLD
&& var < INS_SONAR_VARIANCE_THRESHOLD
#endif
&& ins_update_on_agl
&& baro.status == BS_RUNNING) {
vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar));
last_offset = vff_offset;
}
else {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
}
/** Navigation function on a circle
*/
static inline bool_t mission_nav_circle(struct _mission_element * el) {
struct EnuCoor_i* center_wp = &(el->element.mission_circle.center.center_i);
int32_t radius = el->element.mission_circle.radius;
//Draw the desired circle for a 'duration' time
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
nav_circle(center_wp, POS_BFP_OF_REAL(radius));
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(center_wp->z), 0.);
if (el->duration > 0. && mission.element_time >= el->duration) {
return FALSE;
}
return TRUE;
}
void ins_update_baro() {
#if CONTROL_USE_VFF
if (altimeter_system_status == STATUS_INITIALIZED) {
uint32_t alt = altimeter_get_altitude();
if (!ins.baro_initialised) {
ins.qfe = alt;
ins.baro_initialised = TRUE;
}
ins.baro_alt = alt - ins.qfe;
float alt_float = POS_FLOAT_OF_BFP(ins.baro_alt);
if (ins.vff_realign) {
ins.vff_realign = FALSE;
ins.qfe = alt;
b2_vff_realign(0.);
}
b2_vff_update(alt_float);
}
#endif
}
/** Navigation function to a single waypoint
*/
static inline bool_t mission_nav_wp(struct _mission_element *el)
{
struct EnuCoor_i *target_wp = &(el->element.mission_wp.wp.wp_i);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav_approaching_from(target_wp, NULL, CARROT)) {
last_mission_wp = *target_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(target_wp, el->duration)) { return FALSE; }
} else { return FALSE; }
}
//Go to Mission Waypoint
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
VECT3_COPY(navigation_target, *target_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);
return TRUE;
}
/** Navigation function along a segment
*/
static inline bool_t mission_nav_segment(struct _mission_element *el)
{
struct EnuCoor_i *from_wp = &(el->element.mission_segment.from.from_i);
struct EnuCoor_i *to_wp = &(el->element.mission_segment.to.to_i);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav_approaching_from(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(to_wp, el->duration)) { return FALSE; }
} else { return FALSE; }
}
//Route Between from-to
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(from_wp, to_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(to_wp->z), 0.);
return TRUE;
}
void ins_update_baro() {
int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute);
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro_pressure;
ins_baro_initialised = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro_pressure;
vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { /* not realigning, so normal update with baro measurement */
ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
vff_update_baro(alt_float);
}
}
}
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
{
float dist_home = sqrtf(dist2_to_home);
float dist_wp = sqrtf(dist2_to_wp);
pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
&block_time, &stage_time,
&dist_home, &dist_wp,
&nav_block, &nav_stage,
&horizontal_mode);
if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
} else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
float cx = POS_FLOAT_OF_BFP(nav_circle_center.x);
float cy = POS_FLOAT_OF_BFP(nav_circle_center.y);
float r = POS_FLOAT_OF_BFP(nav_circle_radius);
pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
}
}
uint8_t emma69(uint8_t waypoint) {
float wp1_x = -1.0;
float wp1_y = 1.0;
float h1 = 0.0;
float wp2_x = -1.0;
float wp2_y = -1.0;
float h2 = 0.0;
float wp3_x = 1.0;
float wp3_y = 1.0;
float h3 = 0.0;
float wp4_x = 1.0;
float wp4_y = -1.0;
float h4 = 0.0;
float dist_threshold = 0.1;
double wps[8] = {wp1_x, wp1_y, wp2_x, wp2_y, wp3_x, wp3_y, wp4_x, wp4_y};
double headings[4] = {h1,h2,h3,h4};
struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
//struct EnuCoor_f *pos = stateGetPositionEnu_f(); // Get your current position
printf("Current pos \t");
printf("posX= %f \t", POS_FLOAT_OF_BFP(pos->x)); //POS_FLOAT_OF_BFP(pos->x)
printf("posY= %f \t", POS_FLOAT_OF_BFP(pos->y)); //POS_FLOAT_OF_BFP(pos->y)
printf("\n");
float wpX = waypoint_get_x(waypoint);
float wpY = waypoint_get_y(waypoint);
printf("Current wp \t");
printf("wpX: %f \t", wpX);
printf("wpY: %f \t", wpY);
printf("\n");
//float dist_curr = POS_FLOAT_OF_BFP((POS_BFP_OF_REAL(wpX) - pos->x)*(POS_BFP_OF_REAL(wpX) - pos->x) + (POS_BFP_OF_REAL(wpY) - pos->y)*(POS_BFP_OF_REAL(wpY) - pos->y)); // POS_BFP_OF_REAL POS_FLOAT_OF_BFP(pos->x)
float dist_curr = (wpX - POS_FLOAT_OF_BFP(pos->x))*(wpX - POS_FLOAT_OF_BFP(pos->x)) + (wpY - POS_FLOAT_OF_BFP(pos->y))*(wpY - POS_FLOAT_OF_BFP(pos->y)); // POS_BFP_OF_REAL POS_FLOAT_OF_BFP(pos->x)
printf("Dist to current wp \t");
printf("dist_curr: %f \t", dist_curr);
printf("\n");
//float dist1 = (wpX - wp1_x)*(wpX - wp1_x) + (wpY - wp1_y)*(wpY - wp1_y); // Dist between current wp and navigation wps
//float dist2 = (wpX - wp2_x)*(wpX - wp2_x) + (wpY - wp2_y)*(wpY - wp2_y);
//float dist3 = (wpX - wp3_x)*(wpX - wp3_x) + (wpY - wp3_y)*(wpY - wp3_y);
//if (dist1 < dist2 && dist1 < dist3){i=1;}
//else if (dist2 < dist1 && dist2 < dist3){i=2;}
//else {i=3;}
if (dist_curr < dist_threshold*dist_threshold){
i = i + 1;
if (i> 4){i=1;}
}
// Set the waypoint to the calculated position
printf("Set waypoint to \t");
printf("i: %d \t", i);
printf("wpsX: %f \t",wps[(i-1)*2]);
printf("wpsY: %f \t",wps[(i-1)*2+1]);
printf("\n");
//struct image_t *img = v4l2_image_get(bebop_front_camera.dev, &img);
//struct a *img = v4l2_image_get(bebop_front_camera.dev, &img);
//struct image_t *img;
//printf("image height: %f \t", image_t.h);
printf("wouter is: %d", color_count);
new_coor.x = POS_BFP_OF_REAL(wps[(i-1)*2]);
new_coor.y = POS_BFP_OF_REAL(wps[(i-1)*2+1]);
new_coor.z = pos->z;
// Set the waypoint to the calculated position
waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y);
// Set heading to requested
nav_set_heading_deg(headings[i-1]);
printf("\n");
printf("\n");
return FALSE;
}
int main()
{
struct LtpDef_i ref;
struct EcefCoor_i ecef_ref;
ecef_ref.x = -241887298;
ecef_ref.y = 538037726;
ecef_ref.z = 241732905;
ltp_def_from_ecef_i(&ref,&ecef_ref);
struct EcefCoor_i pos1;
pos1.x = -241887285;
pos1.y = 538037716;
pos1.z = 241732895;
struct NedCoor_i result1;
ned_of_ecef_pos_i(&result1,&ref,&ecef_ref);
printf("x %f, ",POS_FLOAT_OF_BFP(result1.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result1.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result1.z));
struct NedCoor_i result;
ned_of_ecef_pos_i(&result,&ref,&pos1);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
struct NedCoor_i ned_tar1, ned_tar2;
ned_tar1.x = 100 + POS_FLOAT_OF_BFP(result.x) * 100;
ned_tar1.y = POS_FLOAT_OF_BFP(result.y) * 100;
ned_tar1.z = 100 + POS_FLOAT_OF_BFP(result.z) * 100;
ned_tar2.x = -300;
ned_tar2.y = 0;
ned_tar2.z = 100;
struct EcefCoor_i ecef_tar1, ecef_tar2;
ecef_of_ned_point_i(&ecef_tar1,&ref,&ned_tar1);
ecef_of_ned_point_i(&ecef_tar2,&ref,&ned_tar2);
printf("ecef_tar1 x %d y %d z %d\n",ecef_tar1.x,ecef_tar1.y,ecef_tar1.z);
printf("ecef_tar2 x %d y %d z %d\n",ecef_tar2.x,ecef_tar2.y,ecef_tar2.z);
ned_of_ecef_pos_i(&result,&ref,&ecef_tar1);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
ned_of_ecef_pos_i(&result,&ref,&ecef_tar2);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
ned_of_ecef_point_i(&result,&ref,&ecef_tar1);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
ned_of_ecef_point_i(&result,&ref,&ecef_tar2);
printf("x %f, ",POS_FLOAT_OF_BFP(result.x));
printf("y %f, ",POS_FLOAT_OF_BFP(result.y));
printf("z %f\n",POS_FLOAT_OF_BFP(result.z));
return 0;
}
