forked from Archive/PX4-Autopilot
Removed some unnecessairy flags, home position back working
This commit is contained in:
parent
fc4be3e728
commit
d962e6c403
|
@ -1683,7 +1683,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* check if gps fix is ok */
|
||||
// XXX magic number
|
||||
float dop_threshold_m = 2.0f;
|
||||
float hdop_threshold_m = 4.0f;
|
||||
float vdop_threshold_m = 8.0f;
|
||||
|
||||
/*
|
||||
* If horizontal dilution of precision (hdop / eph)
|
||||
|
@ -1694,8 +1695,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
* the system is currently not armed, set home
|
||||
* position to the current position.
|
||||
*/
|
||||
if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m)
|
||||
&& (vdop_m < dop_threshold_m)
|
||||
|
||||
if (gps_position.fix_type == GPS_FIX_TYPE_3D
|
||||
&& (hdop_m < hdop_threshold_m)
|
||||
&& (vdop_m < vdop_threshold_m)
|
||||
&& !home_position_set
|
||||
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
|
||||
&& !current_status.flag_system_armed) {
|
||||
|
|
|
@ -591,7 +591,7 @@ start(const char *uart_path)
|
|||
fd = open(GPS_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("Could not open device path: %s\n", GPS_DEVICE_PATH);
|
||||
errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
|
||||
goto fail;
|
||||
}
|
||||
exit(0);
|
||||
|
|
|
@ -50,12 +50,7 @@
|
|||
|
||||
UBX::UBX() :
|
||||
_config_state(UBX_CONFIG_STATE_PRT),
|
||||
_waiting_for_ack(false),
|
||||
_new_nav_posllh(false),
|
||||
_new_nav_timeutc(false),
|
||||
//_new_nav_dop(false),
|
||||
_new_nav_sol(false),
|
||||
_new_nav_velned(false)
|
||||
_waiting_for_ack(false)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
@ -413,13 +408,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
gps_position->lon = packet->lon;
|
||||
gps_position->alt = packet->height_msl;
|
||||
|
||||
gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m
|
||||
gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m
|
||||
gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
|
||||
gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
|
||||
|
||||
/* Add timestamp to finish the report */
|
||||
gps_position->timestamp_position = hrt_absolute_time();
|
||||
|
||||
_new_nav_posllh = true;
|
||||
/* set flag to trigger publishing of new position */
|
||||
pos_updated = true;
|
||||
|
||||
|
@ -445,9 +439,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
|
||||
gps_position->timestamp_variance = hrt_absolute_time();
|
||||
|
||||
|
||||
_new_nav_sol = true;
|
||||
|
||||
} else {
|
||||
warnx("NAV_SOL: checksum invalid");
|
||||
}
|
||||
|
@ -502,8 +493,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
|
||||
gps_position->timestamp_time = hrt_absolute_time();
|
||||
|
||||
_new_nav_timeutc = true;
|
||||
|
||||
} else {
|
||||
warnx("NAV_TIMEUTC: checksum invalid");
|
||||
}
|
||||
|
@ -581,20 +570,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
gps_position->satellite_elevation[i] = 0;
|
||||
gps_position->satellite_azimuth[i] = 0;
|
||||
}
|
||||
gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
|
||||
|
||||
/* set flag if any sat info is available */
|
||||
if (!packet_part1->numCh > 0) {
|
||||
gps_position->satellite_info_available = 1;
|
||||
|
||||
/* set timestamp if any sat info is available */
|
||||
if (packet_part1->numCh > 0) {
|
||||
gps_position->satellite_info_available = true;
|
||||
} else {
|
||||
gps_position->satellite_info_available = 0;
|
||||
gps_position->satellite_info_available = false;
|
||||
}
|
||||
|
||||
gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones
|
||||
gps_position->timestamp_satellites = hrt_absolute_time();
|
||||
|
||||
// as this message arrives only with 1Hz and is not essential, we don't take it into account for the report
|
||||
|
||||
} else {
|
||||
warnx("NAV_SVINFO: checksum invalid");
|
||||
}
|
||||
|
@ -619,9 +604,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
gps_position->vel_ned_valid = true;
|
||||
gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
|
||||
_new_nav_velned = true;
|
||||
|
||||
|
||||
} else {
|
||||
warnx("NAV_VELNED: checksum invalid");
|
||||
}
|
||||
|
@ -760,21 +742,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* return 1 when position updates and the remaining packets updated at least once */
|
||||
if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) {
|
||||
|
||||
/* we have received everything, this most probably means that the configuration is fine */
|
||||
config_needed = false;
|
||||
|
||||
/* Reset the flags */
|
||||
_new_nav_posllh = false;
|
||||
_new_nav_timeutc = false;
|
||||
// _new_nav_dop = false;
|
||||
_new_nav_sol = false;
|
||||
_new_nav_velned = false;
|
||||
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
|
@ -369,11 +369,6 @@ private:
|
|||
ubx_message_class_t _message_class;
|
||||
ubx_message_id_t _message_id;
|
||||
unsigned _payload_size;
|
||||
bool _new_nav_posllh;
|
||||
bool _new_nav_timeutc;
|
||||
// bool _new_nav_dop;
|
||||
bool _new_nav_sol;
|
||||
bool _new_nav_velned;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
|
|
@ -86,7 +86,7 @@ struct vehicle_gps_position_s
|
|||
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
|
||||
uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */
|
||||
bool satellite_info_available; /**< 0 for no info, 1 for info available */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue