dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Removed some unnecessairy flags, home position back working

This commit is contained in:
Julian Oes 2013-02-06 13:50:32 -08:00
parent fc4be3e728
commit d962e6c403
5 changed files with 16 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
apps/commander/commander.c
View File

@ -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) {

2
apps/drivers/gps/gps.cpp
View File

@ -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);

49
apps/drivers/gps/ubx.cpp
View File

@ -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;
}

5
apps/drivers/gps/ubx.h
View File

@ -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_ */

2
apps/uORB/topics/vehicle_gps_position.h
View File

@ -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 */
};
/**