forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into hkmicropcb
This commit is contained in:
Thomas Gubler
commit
f6406a0b19
|
|
@ -0,0 +1,44 @@
|
|||
# Contributing to PX4 Firmware
|
||||
|
||||
We follow the [Github flow](https://guides.github.com/introduction/flow/) development model.
|
||||
|
||||
### Fork the project, then clone your repo
|
||||
|
||||
First [fork and clone](https://help.github.com/articles/fork-a-repo) the project project.
|
||||
|
||||
### Create a feature branch
|
||||
|
||||
*Always* branch off master for new features.
|
||||
|
||||
```
|
||||
git checkout -b mydescriptivebranchname
|
||||
```
|
||||
|
||||
### Edit and build the code
|
||||
|
||||
The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files.
|
||||
|
||||
### Commit your changes
|
||||
|
||||
Always write descriptive commit messages and add a fixes or relates note to them with an [issue number](https://github.com/px4/Firmware/issues) (Github will link these then conveniently)
|
||||
|
||||
**Example:**
|
||||
|
||||
```
|
||||
Change how the attitude controller works
|
||||
|
||||
- Fixes rate feed forward
|
||||
- Allows a local body rate override
|
||||
|
||||
Fixes issue #123
|
||||
```
|
||||
|
||||
### Test your changes
|
||||
|
||||
Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the logfile from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link.
|
||||
|
||||
### Push your changes
|
||||
|
||||
Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/).
|
||||
|
||||
Make sure to provide some testing feedback and if possible the link to a flight log file.
|
||||
|
|
@ -299,7 +299,6 @@ GPS::task_main()
|
|||
_report_gps_pos.alt = (int32_t)1200e3f;
|
||||
_report_gps_pos.timestamp_variance = hrt_absolute_time();
|
||||
_report_gps_pos.s_variance_m_s = 10.0f;
|
||||
_report_gps_pos.p_variance_m = 10.0f;
|
||||
_report_gps_pos.c_variance_rad = 0.1f;
|
||||
_report_gps_pos.fix_type = 3;
|
||||
_report_gps_pos.eph = 0.9f;
|
||||
|
|
|
|||
|
|
@ -95,7 +95,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate
|
|||
_got_velned(false),
|
||||
_disable_cmd_last(0),
|
||||
_ack_waiting_msg(0),
|
||||
_ubx_version(0)
|
||||
_ubx_version(0),
|
||||
_use_nav_pvt(false)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
|
|
@ -190,38 +191,45 @@ UBX::configure(unsigned &baudrate)
|
|||
|
||||
/* configure message rates */
|
||||
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
|
||||
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
|
||||
|
||||
/* try to set rate for NAV-PVT */
|
||||
/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
|
||||
configure_message_rate(UBX_MSG_NAV_PVT, 1);
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
_use_nav_pvt = false;
|
||||
} else {
|
||||
_use_nav_pvt = true;
|
||||
}
|
||||
UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
|
||||
if (!_use_nav_pvt) {
|
||||
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_SOL, 1);
|
||||
configure_message_rate(UBX_MSG_NAV_SOL, 1);
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_MON_HW, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
|
@ -454,11 +462,23 @@ UBX::payload_rx_init()
|
|||
_rx_state = UBX_RXMSG_HANDLE; // handle by default
|
||||
|
||||
switch (_rx_msg) {
|
||||
case UBX_MSG_NAV_PVT:
|
||||
if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */
|
||||
&& (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (!_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_POSLLH:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_SOL:
|
||||
|
|
@ -466,6 +486,8 @@ UBX::payload_rx_init()
|
|||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_TIMEUTC:
|
||||
|
|
@ -473,6 +495,8 @@ UBX::payload_rx_init()
|
|||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_SVINFO:
|
||||
|
|
@ -489,6 +513,8 @@ UBX::payload_rx_init()
|
|||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
break;
|
||||
|
||||
case UBX_MSG_MON_VER:
|
||||
|
|
@ -675,6 +701,68 @@ UBX::payload_rx_done(void)
|
|||
// handle message
|
||||
switch (_rx_msg) {
|
||||
|
||||
case UBX_MSG_NAV_PVT:
|
||||
UBX_TRACE_RXMSG("Rx NAV-PVT\n");
|
||||
|
||||
_gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType;
|
||||
_gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV;
|
||||
|
||||
_gps_position->lat = _buf.payload_rx_nav_pvt.lat;
|
||||
_gps_position->lon = _buf.payload_rx_nav_pvt.lon;
|
||||
_gps_position->alt = _buf.payload_rx_nav_pvt.hMSL;
|
||||
|
||||
_gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f;
|
||||
_gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
|
||||
_gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
|
||||
|
||||
_gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
|
||||
|
||||
_gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
|
||||
_gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f;
|
||||
_gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f;
|
||||
_gps_position->vel_ned_valid = true;
|
||||
|
||||
_gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
|
||||
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
|
||||
|
||||
{
|
||||
/* convert to unix timestamp */
|
||||
struct tm timeinfo;
|
||||
timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900;
|
||||
timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1;
|
||||
timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day;
|
||||
timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour;
|
||||
timeinfo.tm_min = _buf.payload_rx_nav_pvt.min;
|
||||
timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec;
|
||||
time_t epoch = mktime(&timeinfo);
|
||||
|
||||
#ifndef CONFIG_RTC
|
||||
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
|
||||
//TODO generalize this by moving into gps.cpp?
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
|
||||
clock_settime(CLOCK_REALTIME, &ts);
|
||||
#endif
|
||||
|
||||
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
|
||||
}
|
||||
|
||||
_gps_position->timestamp_time = hrt_absolute_time();
|
||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||
_gps_position->timestamp_position = hrt_absolute_time();
|
||||
|
||||
_rate_count_vel++;
|
||||
_rate_count_lat_lon++;
|
||||
|
||||
_got_posllh = true;
|
||||
_got_velned = true;
|
||||
|
||||
ret = 1;
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_POSLLH:
|
||||
UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
|
||||
|
||||
|
|
@ -697,7 +785,6 @@ UBX::payload_rx_done(void)
|
|||
|
||||
_gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix;
|
||||
_gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m
|
||||
_gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m
|
||||
_gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV;
|
||||
|
||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||
|
|
|
|||
|
|
@ -104,6 +104,13 @@
|
|||
#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */
|
||||
#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */
|
||||
|
||||
/* RX NAV-TIMEUTC message content details */
|
||||
/* Bitfield "valid" masks */
|
||||
#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW 0x01 /**< validTOW (1 = Valid Time of Week) */
|
||||
#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN 0x02 /**< validWKN (1 = Valid Week Number) */
|
||||
#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC 0x04 /**< validUTC (1 = Valid UTC Time) */
|
||||
#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD 0xF0 /**< utcStandard (0..15 = UTC standard identifier) */
|
||||
|
||||
/* TX CFG-PRT message contents */
|
||||
#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */
|
||||
#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
||||
|
|
@ -176,7 +183,7 @@ typedef struct {
|
|||
uint32_t reserved2;
|
||||
} ubx_payload_rx_nav_sol_t;
|
||||
|
||||
/* Rx NAV-PVT */
|
||||
/* Rx NAV-PVT (ubx8) */
|
||||
typedef struct {
|
||||
uint32_t iTOW; /**< GPS Time of Week [ms] */
|
||||
uint16_t year; /**< Year (UTC)*/
|
||||
|
|
@ -185,7 +192,7 @@ typedef struct {
|
|||
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
|
||||
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
|
||||
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
|
||||
uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
|
||||
uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
|
||||
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
|
||||
int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */
|
||||
uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
|
||||
|
|
@ -208,9 +215,11 @@ typedef struct {
|
|||
uint16_t pDOP; /**< Position DOP [0.01] */
|
||||
uint16_t reserved2;
|
||||
uint32_t reserved3;
|
||||
int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */
|
||||
uint32_t reserved4;
|
||||
int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
|
||||
uint32_t reserved4; /**< (ubx8+ only) */
|
||||
} ubx_payload_rx_nav_pvt_t;
|
||||
#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8)
|
||||
#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t))
|
||||
|
||||
/* Rx NAV-TIMEUTC */
|
||||
typedef struct {
|
||||
|
|
@ -223,7 +232,7 @@ typedef struct {
|
|||
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
|
||||
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
|
||||
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
|
||||
uint8_t valid; /**< Validity Flags (see ubx documentation) */
|
||||
uint8_t valid; /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
|
||||
} ubx_payload_rx_nav_timeutc_t;
|
||||
|
||||
/* Rx NAV-SVINFO Part 1 */
|
||||
|
|
@ -388,6 +397,7 @@ typedef struct {
|
|||
|
||||
/* General message and payload buffer union */
|
||||
typedef union {
|
||||
ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt;
|
||||
ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh;
|
||||
ubx_payload_rx_nav_sol_t payload_rx_nav_sol;
|
||||
ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc;
|
||||
|
|
@ -526,6 +536,7 @@ private:
|
|||
uint16_t _ack_waiting_msg;
|
||||
ubx_buf_t _buf;
|
||||
uint32_t _ubx_version;
|
||||
bool _use_nav_pvt;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
|
|
|||
|
|
@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
|||
|
||||
// Calculate initial Tbn matrix and rotate Mag measurements into NED
|
||||
// to set initial NED magnetic field states
|
||||
Mat3f DCM;
|
||||
quat2Tbn(DCM, initQuat);
|
||||
quat2Tbn(Tbn, initQuat);
|
||||
Tnb = Tbn.transpose();
|
||||
Vector3f initMagNED;
|
||||
initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
|
||||
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
|
||||
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
|
||||
initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
|
||||
initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
|
||||
initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
|
||||
|
||||
magstate.q0 = initQuat[0];
|
||||
magstate.q1 = initQuat[1];
|
||||
|
|
@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
|||
magstate.magYbias = magBias.y;
|
||||
magstate.magZbias = magBias.z;
|
||||
magstate.R_MAG = sq(magMeasurementSigma);
|
||||
magstate.DCM = DCM;
|
||||
magstate.DCM = Tbn;
|
||||
|
||||
// write to state vector
|
||||
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
|
||||
|
|
|
|||
|
|
@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
|||
|
||||
hil_gps.timestamp_variance = timestamp;
|
||||
hil_gps.s_variance_m_s = 5.0f;
|
||||
hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
|
||||
|
||||
hil_gps.timestamp_velocity = timestamp;
|
||||
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
|
||||
|
|
|
|||
|
|
@ -47,8 +47,8 @@
|
|||
#include "px4io.h"
|
||||
|
||||
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
|
||||
#define RC_CHANNEL_HIGH_THRESH 5000
|
||||
#define RC_CHANNEL_LOW_THRESH -5000
|
||||
#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */
|
||||
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
|
||||
|
||||
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
|
||||
|
||||
|
|
|
|||
|
|
@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args)
|
|||
fprintf(stderr, "\n");
|
||||
#elif CONFIG_ARCH_LOWPUTC
|
||||
lowsyslog("%s: ", getprogname());
|
||||
lowvyslog(fmt, args);
|
||||
lowvsyslog(fmt, args);
|
||||
|
||||
/* convenience as many parts of NuttX use negative errno */
|
||||
if (errcode < 0)
|
||||
|
|
|
|||
|
|
@ -61,7 +61,6 @@ struct vehicle_gps_position_s {
|
|||
|
||||
uint64_t timestamp_variance;
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float p_variance_m; /**< position accuracy estimate m */
|
||||
float c_variance_rad; /**< course accuracy estimate rad */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue