// Called on each vision analysis result after receiving the struct
void run_avoid_navigation_onvision(void)
{
// Send ALL vision data to the ground
DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin);
switch (avoid_navigation_data.mode) {
case 0: // Go to Goal and stop at obstacles
//count 4 subsequent obstacles
if (avoid_navigation_data.stereo_bin[0] > 1) {
counter = counter + 1;
if (counter > 1) {
counter = 0;
//Obstacle detected, go to turn until clear mode
obstacle_detected = TRUE;
avoid_navigation_data.mode = 1;
}
} else {
counter = 0;
}
break;
case 1: // Turn until clear
//count 20 subsequent free frames
if (avoid_navigation_data.stereo_bin[0] < 1) {
counter = counter + 1;
if (counter > 12) {
counter = 0;
//Stop and put waypoint 2.5 m ahead
struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i();
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
new_coor.z = pos->z;
waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y);
obstacle_detected = FALSE;
avoid_navigation_data.mode = 0;
}
} else {
counter = 0;
}
break;
case 2:
break;
default: // do nothing
break;
}
avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0] > 20;
avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode;
avoid_navigation_data.stereo_bin[4] = counter;
#ifdef STEREO_LED
if (obstacle_detected) {
LED_ON(STEREO_LED);
} else {
LED_OFF(STEREO_LED);
}
#endif
}
/** update ins state from horizontal filter */
static void ins_update_from_hff(void)
{
ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
/** update local ENU coordinates from its LLA coordinates */
void waypoint_localize(uint8_t wp_id)
{
if (state.ned_initialized_i) {
struct EnuCoor_i enu;
enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla);
// convert ENU pos from cm to BFP with INT32_POS_FRAC
enu.x = POS_BFP_OF_REAL(enu.x) / 100;
enu.y = POS_BFP_OF_REAL(enu.y) / 100;
enu.z = POS_BFP_OF_REAL(enu.z) / 100;
waypoints[wp_id].enu_i = enu;
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
}
}
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);
}
}
static inline void nav_set_altitude( void ) {
static int32_t last_nav_alt = 0;
if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
nav_flight_altitude = nav_altitude;
last_nav_alt = nav_altitude;
}
}
void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_meas_ltp;
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
#if USE_VFF
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
if (baro.status == BS_RUNNING && ins_baro_initialised) {
vff_propagate(z_accel_meas_float);
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 { // feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#else
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
#endif /* USE_VFF */
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
ins_ltp_accel.x = accel_meas_ltp.x;
ins_ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
INS_NED_TO_STATE();
}
void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_meas_ltp;
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
if (ins_impl.baro_initialized) {
vff_propagate(z_accel_meas_float);
ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity,
// but vehicle not accelerating in ltp)
ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
/* convert and copy result to ins_impl */
ins_update_from_hff();
#else
ins_impl.ltp_accel.x = accel_meas_ltp.x;
ins_impl.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
ins_ned_to_state();
}
void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_body;
INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_ltp;
INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
#ifdef USE_VFF
if (baro.status == BS_RUNNING && ins_baro_initialised) {
vff_propagate(z_accel_float);
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 { // feed accel from the sensors
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
}
#else
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
#endif /* USE_VFF */
#ifdef USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
ins_ltp_accel.x = accel_ltp.x;
ins_ltp_accel.y = accel_ltp.y;
#endif /* USE_HFF */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
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 nav_init(void)
{
waypoints_init();
nav_block = 0;
nav_stage = 0;
nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
nav_flight_altitude = nav_altitude;
flight_altitude = SECURITY_ALT;
VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
vertical_mode = VERTICAL_MODE_ALT;
nav_roll = 0;
nav_pitch = 0;
nav_heading = 0;
nav_radius = DEFAULT_CIRCLE_RADIUS;
nav_throttle = 0;
nav_climb = 0;
nav_leg_progress = 0;
nav_leg_length = 1;
too_far_from_home = FALSE;
dist2_to_home = 0;
dist2_to_wp = 0;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status);
register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
#endif
}
/* Command The Camera */
void dc_send_command(uint8_t cmd)
{
switch (cmd) {
case DC_SHOOT:
// Send Photo Position To Camera
dc_shot_msg.data.nr = dc_photo_nr + 1;
dc_shot_msg.data.lat = stateGetPositionLla_i()->lat;
dc_shot_msg.data.lon = stateGetPositionLla_i()->lon;
dc_shot_msg.data.alt = stateGetPositionLla_i()->alt;
dc_shot_msg.data.phi = stateGetNedToBodyEulers_i()->phi;
dc_shot_msg.data.theta = stateGetNedToBodyEulers_i()->theta;
dc_shot_msg.data.psi = stateGetNedToBodyEulers_i()->psi;
dc_shot_msg.data.vground = *stateGetHorizontalSpeedNorm_i();
dc_shot_msg.data.course = *stateGetHorizontalSpeedDir_i();
dc_shot_msg.data.groundalt = POS_BFP_OF_REAL(state.alt_agl_f);
MoraHeader(MORA_SHOOT, MORA_SHOOT_MSG_SIZE);
for (int i = 0; i < (MORA_SHOOT_MSG_SIZE); i++) {
MoraPutUint8(dc_shot_msg.bin[i]);
}
MoraTrailer();
dc_send_shot_position();
break;
case DC_TALLER:
break;
case DC_WIDER:
break;
case DC_ON:
break;
case DC_OFF:
break;
default:
break;
}
}
void waypoint_set_alt(uint8_t wp_id, float alt)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_f.z = alt;
/* also update ENU fixed point representation */
waypoints[wp_id].enu_i.z = POS_BFP_OF_REAL(waypoints[wp_id].enu_f.z);
waypoint_globalize(wp_id);
}
}
uint8_t moveWaypointForwards(uint8_t waypoint, float distanceMeters)
{
struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
// Calculate the sine and cosine of the heading the drone is keeping
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
// Now determine where to place the waypoint you want to go to
new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (distanceMeters));
new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (distanceMeters));
new_coor.z = pos->z; // Keep the height the same
// Set the waypoint to the calculated position
waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y);
return false;
}
void nav_init(void) {
// convert to
const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU;
// init int32 waypoints
uint8_t i = 0;
for (i = 0; i < nb_waypoint; i++) {
waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x);
waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y);
waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z);
}
nav_block = 0;
nav_stage = 0;
nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
nav_flight_altitude = nav_altitude;
flight_altitude = SECURITY_ALT;
VECT3_COPY(navigation_target, waypoints[WP_HOME]);
VECT3_COPY(navigation_carrot, waypoints[WP_HOME]);
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
vertical_mode = VERTICAL_MODE_ALT;
nav_roll = 0;
nav_pitch = 0;
nav_heading = 0;
nav_radius = DEFAULT_CIRCLE_RADIUS;
nav_throttle = 0;
nav_climb = 0;
nav_leg_progress = 0;
nav_leg_length = 1;
too_far_from_home = FALSE;
dist2_to_home = 0;
dist2_to_wp = 0;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status);
register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
#endif
}
void follow_change_wp( unsigned char* buffer ) {
struct EcefCoor_i new_pos;
struct EnuCoor_i enu;
new_pos.x = DL_REMOTE_GPS_ecef_x(buffer);
new_pos.y = DL_REMOTE_GPS_ecef_y(buffer);
new_pos.z = DL_REMOTE_GPS_ecef_z(buffer);
// Translate to ENU
enu_of_ecef_point_i(&enu, &ins_impl.ltp_def, &new_pos);
INT32_VECT3_SCALE_2(enu, enu, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
// TODO: Add the angle to the north
// Update the offsets
enu.x += POS_BFP_OF_REAL(FOLLOW_OFFSET_X);
enu.y += POS_BFP_OF_REAL(FOLLOW_OFFSET_Y);
enu.z += POS_BFP_OF_REAL(FOLLOW_OFFSET_Z);
// TODO: Remove the angle to the north
// Move the waypoint
VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu);
}
/** 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_propagate() {
#if CONTROL_USE_VFF
if (altimeter_system_status == STATUS_INITIALIZED && ins.baro_initialised) {
float accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
b2_vff_propagate(accel_float);
ins.ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
ins.ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
ins.ltp_pos.z = POS_BFP_OF_REAL(b2_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;
}
#endif
#ifdef USE_HFF
if (booz_ahrs.status == BOOZ_AHRS_RUNNING &&
gps_state.fix == BOOZ2_GPS_FIX_3D && ins.ltp_initialised )
b2ins_propagate();
#endif
}
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);
}
}
}
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING:
{
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING :
{
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING :
{
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
/* this case for DL_HOVER_POINT_CHANGE_ECEF is added by Yi and Edward */
/*********************************************************************/
case DL_HOVER_POINT_CHANGE_ECEF:
{
uint8_t ac_id = DL_HOVER_POINT_CHANGE_ECEF_ac_id(dl_buffer);
if(ac_id != AC_ID) break;
uint8_t external_gps_fixed = DL_HOVER_POINT_CHANGE_ECEF_external_gps_fix(dl_buffer);
/** getting the ecef coordinate from external gps **/
struct EcefCoor_i external_gps_ecef_pos;
external_gps_ecef_pos.x = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x(dl_buffer);//DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x() is defined in dl_protocol.h
external_gps_ecef_pos.y = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_y(dl_buffer);
external_gps_ecef_pos.z = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_z(dl_buffer);
DOWNLINK_SEND_HOVER_POINT_CHANGE_ECEF(DefaultChannel, DefaultDevice, &ac_id, &external_gps_fixed, &(external_gps_ecef_pos.x), &(external_gps_ecef_pos.y), &(external_gps_ecef_pos.z));
if(external_gps_fixed == 0x03)//case for 3D_Fix.
{
if(!ins_ltp_initialised || guidance_h_mode != GUIDANCE_H_MODE_HOVER) break;//ins_ltp_initialised defined in ins_int.h
//no HMOE reference
//not in the HOVER_Z_HOLD mode
//not in flight(looks like no needed)
struct NedCoor_i external_gps_ned_pos;
memset((void *) &external_gps_ned_pos,0,sizeof(struct NedCoor_i));//just in case, clear the memory.
/** convert the ecef coordinate into ned(ltp) coordinate **/
ned_of_ecef_point_i(&external_gps_ned_pos,&ins_ltp_def,&external_gps_ecef_pos);
/** change the current setpoint with external_gps_ned_pos coordinate **/
struct Int32Vect2 external_gps_h_pos;
//cm to m and store in external_gps_h_pos
//check function ins_update_gps() for reference.
INT32_VECT2_SCALE_2(external_gps_h_pos,external_gps_ned_pos,INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
//subsitute the guidance_h_pos_sp
VECT2_COPY(guidance_h_pos_sp,external_gps_h_pos);
/*
* the above code is used for congruity, but waste space and time.
* simple using
*
* guidance_h_pos_sp.x = external_gps_ned_pos.x;
* guidance_h_pos_sp.y = external_gps_ned_pox.y;
*
* can achieve the same effect.
*/
}
else break;//hover pos_sp no change
}
/**------------------------------------------------------------------*/
#if defined USE_NAVIGATION
case DL_BLOCK :
{
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP :
{
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) break;
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
/* WP_alt is in cm, lla.alt in mm */
lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
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;
}
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
}
//TODO use datalink parsing from rotorcraft instead of doing it here explicitly
#include "generated/settings.h"
#include "dl_protocol.h"
#include "subsystems/datalink/downlink.h"
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
}
}
void b2_hff_propagate(void) {
if (b2_hff_lost_counter < b2_hff_lost_limit)
b2_hff_lost_counter++;
#ifdef GPS_LAG
/* continue re-propagating to catch up with the present */
if (b2_hff_rb_last->rollback) {
b2_hff_propagate_past(b2_hff_rb_last);
}
#endif
/* store body accelerations for mean computation */
b2_hff_store_accel_body();
/* propagate current state if it is time */
if (b2_hff_ps_counter == HFF_PRESCALER) {
b2_hff_ps_counter = 1;
if (b2_hff_lost_counter < b2_hff_lost_limit) {
/* compute float ltp mean acceleration */
b2_hff_compute_accel_body_mean(HFF_PRESCALER);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean);
b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif
/*
* propagate current state
*/
b2_hff_propagate_x(&b2_hff_state);
b2_hff_propagate_y(&b2_hff_state);
/* update ins state from horizontal filter */
ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifdef GPS_LAG
/* increase lag counter on last saved state */
if (b2_hff_rb_n > 0)
b2_hff_rb_last->lag_counter++;
/* save filter state if needed */
if (save_counter == 0) {
PRINT_DBG(1, ("save current state\n"));
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
save_counter--;
}
#endif
}
} else {
b2_hff_ps_counter++;
}
}
void b2_hff_update_gps(void) {
b2_hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
if (Rgps_pos < HFF_R_POS_MIN)
Rgps_pos = HFF_R_POS_MIN;
Rgps_vel = (float) gps.sacc / 100.;
if (Rgps_vel < HFF_R_SPEED_MIN)
Rgps_vel = HFF_R_SPEED_MIN;
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
/* update ins state */
ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifdef GPS_LAG
} else if (b2_hff_rb_n > 0) {
/* roll back if state was saved approx when GPS was valid */
lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N-1;// + lag_counter_err;
PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
b2_hff_propagate_past(b2_hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N+1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps();
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#endif /* GPS_LAG */
}
/** update ins state from vertical filter */
static void ins_update_from_vff(void)
{
ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot);
ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot);
ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff.z);
}
