forked from Archive/PX4-Autopilot
Merge branch 'master' into offboard2_merge
Conflicts: src/modules/commander/commander.cpp
This commit is contained in:
Julian Oes
commit
79cbf15d26
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#!nsh
|
||||
#
|
||||
# Hobbyking Micro Integrated PCB Quadcopter
|
||||
# with SimonK ESC firmware and Mystery A1510 motors
|
||||
#
|
||||
# Thomas Gubler <thomasgubler@gmail.com>
|
||||
#
|
||||
echo "HK Micro PCB Quad"
|
||||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
param set MC_ROLLRATE_I 0.0
|
||||
param set MC_ROLLRATE_D 0.003
|
||||
param set MC_PITCH_P 7.0
|
||||
param set MC_PITCHRATE_P 0.1
|
||||
param set MC_PITCHRATE_I 0.0
|
||||
param set MC_PITCHRATE_D 0.003
|
||||
param set MC_YAW_P 2.8
|
||||
param set MC_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_YAWRATE_D 0.0
|
||||
fi
|
||||
|
||||
set PWM_MIN 1200
|
||||
|
|
@ -136,6 +136,11 @@ then
|
|||
sh /etc/init.d/4011_dji_f450
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 4020
|
||||
then
|
||||
sh /etc/init.d/4020_hk_micro_pcb
|
||||
fi
|
||||
|
||||
#
|
||||
# Quad +
|
||||
#
|
||||
|
|
|
|||
|
|
@ -33,7 +33,6 @@ MODULES += drivers/hil
|
|||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ MODULES += drivers/l3gd20
|
|||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/pca8574
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/tests
|
||||
|
|
|
|||
|
|
@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) :
|
|||
safety_sub_fd(-1),
|
||||
num_of_cells(0),
|
||||
detected_cells_runcount(0),
|
||||
t_led_color({0}),
|
||||
t_led_color{0},
|
||||
t_led_blink(0),
|
||||
led_thread_runcount(0),
|
||||
led_interval(1000),
|
||||
|
|
|
|||
|
|
@ -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_ */
|
||||
|
|
|
|||
|
|
@ -294,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
|||
// calculate the position of the start and end points. We should not be doing this often
|
||||
// as this function generally will not be called repeatedly when we are out of the sector.
|
||||
|
||||
// TO DO - this is messed up and won't compile
|
||||
float start_disp_x = radius * sinf(arc_start_bearing);
|
||||
float start_disp_y = radius * cosf(arc_start_bearing);
|
||||
float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
|
||||
float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
|
||||
float lon_start = lon_now + start_disp_x / 111111.0f;
|
||||
float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
|
||||
float lon_end = lon_now + end_disp_x / 111111.0f;
|
||||
float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
|
||||
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
double start_disp_x = (double)radius * sin(arc_start_bearing);
|
||||
double start_disp_y = (double)radius * cos(arc_start_bearing);
|
||||
double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double lon_start = lon_now + start_disp_x / 111111.0;
|
||||
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
|
||||
double lon_end = lon_now + end_disp_x / 111111.0;
|
||||
double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
|
||||
double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
|
||||
|
||||
if (dist_to_start < dist_to_end) {
|
||||
crosstrack_error->distance = dist_to_start;
|
||||
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
|
||||
} else {
|
||||
crosstrack_error->past_end = true;
|
||||
crosstrack_error->distance = dist_to_end;
|
||||
|
|
@ -319,7 +317,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
|||
|
||||
}
|
||||
|
||||
crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
|
||||
crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
|
||||
return_value = OK;
|
||||
return return_value;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -478,7 +478,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
|
||||
// We use an float epsilon delta to test float equality.
|
||||
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
|
||||
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
|
||||
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1);
|
||||
|
||||
} else {
|
||||
|
||||
|
|
@ -640,7 +640,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* welcome user */
|
||||
warnx("starting");
|
||||
|
||||
char *main_states_str[MAIN_STATE_MAX];
|
||||
const char *main_states_str[MAIN_STATE_MAX];
|
||||
main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
|
||||
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
|
||||
main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
|
||||
|
|
@ -650,7 +650,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
main_states_str[MAIN_STATE_ACRO] = "ACRO";
|
||||
main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD";
|
||||
|
||||
char *arming_states_str[ARMING_STATE_MAX];
|
||||
const char *arming_states_str[ARMING_STATE_MAX];
|
||||
arming_states_str[ARMING_STATE_INIT] = "INIT";
|
||||
arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
|
||||
arming_states_str[ARMING_STATE_ARMED] = "ARMED";
|
||||
|
|
@ -659,7 +659,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
|
||||
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
|
||||
|
||||
char *nav_states_str[NAVIGATION_STATE_MAX];
|
||||
const char *nav_states_str[NAVIGATION_STATE_MAX];
|
||||
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
|
||||
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
|
||||
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
|
||||
|
|
@ -781,7 +781,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
bool updated = false;
|
||||
|
||||
bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
rc_calibration_check(mavlink_fd);
|
||||
|
||||
/* Subscribe to safety topic */
|
||||
int safety_sub = orb_subscribe(ORB_ID(safety));
|
||||
|
|
@ -803,10 +803,16 @@ int commander_thread_main(int argc, char *argv[])
|
|||
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
memset(&sp_offboard, 0, sizeof(sp_offboard));
|
||||
|
||||
/* Subscribe to telemetry status */
|
||||
int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
struct telemetry_status_s telemetry;
|
||||
memset(&telemetry, 0, sizeof(telemetry));
|
||||
/* Subscribe to telemetry status topics */
|
||||
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
|
||||
telemetry_last_heartbeat[i] = 0;
|
||||
telemetry_lost[i] = true;
|
||||
}
|
||||
|
||||
/* Subscribe to global position */
|
||||
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
|
|
@ -888,7 +894,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
bool arming_state_changed = false;
|
||||
bool main_state_changed = false;
|
||||
bool failsafe_old = false;
|
||||
bool system_checked = false;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
|
|
@ -936,7 +941,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_changed = true;
|
||||
|
||||
/* re-check RC calibration */
|
||||
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
|
||||
/* navigation parameters */
|
||||
|
|
@ -945,15 +950,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
|
||||
}
|
||||
|
||||
/* Perform system checks (again) once params are loaded and MAVLink is up. */
|
||||
if (!system_checked && mavlink_fd &&
|
||||
(telemetry.heartbeat_time > 0) &&
|
||||
(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
|
||||
|
||||
(void)rc_calibration_check(mavlink_fd);
|
||||
system_checked = true;
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
|
|
@ -979,10 +975,26 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
orb_check(telemetry_sub, &updated);
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
orb_check(telemetry_subs[i], &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
|
||||
if (updated) {
|
||||
struct telemetry_status_s telemetry;
|
||||
memset(&telemetry, 0, sizeof(telemetry));
|
||||
|
||||
orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
|
||||
|
||||
/* perform system checks when new telemetry link connected */
|
||||
if (mavlink_fd &&
|
||||
telemetry_last_heartbeat[i] == 0 &&
|
||||
telemetry.heartbeat_time > 0 &&
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
|
||||
|
||||
(void)rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
|
||||
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
|
||||
}
|
||||
}
|
||||
|
||||
orb_check(sensor_sub, &updated);
|
||||
|
|
@ -1386,18 +1398,35 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
/* data link check */
|
||||
if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
|
||||
/* data links check */
|
||||
bool have_link = false;
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
|
||||
/* handle the case where data link was regained */
|
||||
if (telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
|
||||
telemetry_lost[i] = false;
|
||||
}
|
||||
have_link = true;
|
||||
|
||||
} else {
|
||||
if (!telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
|
||||
telemetry_lost[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (have_link) {
|
||||
/* handle the case where data link was regained */
|
||||
if (status.data_link_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link regained");
|
||||
status.data_link_lost = false;
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.data_link_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
|
||||
status.data_link_lost = true;
|
||||
status_changed = true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1533,7 +1533,7 @@ FixedwingEstimator::start()
|
|||
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
6000,
|
||||
5000,
|
||||
(main_t)&FixedwingEstimator::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
_attitude_sp_pub(-1),
|
||||
_nav_capabilities_pub(-1),
|
||||
|
||||
_att(),
|
||||
_att_sp(),
|
||||
_nav_capabilities(),
|
||||
_manual(),
|
||||
_airspeed(),
|
||||
_control_mode(),
|
||||
_global_pos(),
|
||||
_pos_sp_triplet(),
|
||||
_sensor_combined(),
|
||||
_range_finder(),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
|
||||
|
||||
|
|
@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
_airspeed_valid(false),
|
||||
_groundspeed_undershoot(0.0f),
|
||||
_global_pos_valid(false),
|
||||
_att(),
|
||||
_att_sp(),
|
||||
_nav_capabilities(),
|
||||
_manual(),
|
||||
_airspeed(),
|
||||
_control_mode(),
|
||||
_global_pos(),
|
||||
_pos_sp_triplet(),
|
||||
_sensor_combined(),
|
||||
_mTecs(),
|
||||
_was_pos_control_mode(false),
|
||||
_range_finder()
|
||||
_was_pos_control_mode(false)
|
||||
{
|
||||
_nav_capabilities.turn_distance = 0.0f;
|
||||
|
||||
|
|
@ -806,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
|
||||
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
|
||||
|
||||
// XXX re-visit
|
||||
float baro_altitude = _global_pos.alt;
|
||||
|
||||
/* filter speed and altitude for controller */
|
||||
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
|
||||
math::Vector<3> accel_earth = _R_nb * accel_body;
|
||||
|
||||
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
|
||||
|
||||
|
|
@ -944,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
float airspeed_land = 1.3f * _parameters.airspeed_min;
|
||||
float airspeed_approach = 1.3f * _parameters.airspeed_min;
|
||||
|
||||
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
|
||||
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
|
||||
/* Calculate altitude of last ordinary waypoint L */
|
||||
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
|
||||
|
||||
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
|
||||
|
|
@ -1449,7 +1445,7 @@ FixedwingPositionControl::start()
|
|||
_control_task = task_spawn_cmd("fw_pos_control_l1",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4048,
|
||||
3500,
|
||||
(main_t)&FixedwingPositionControl::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
|
|
|||
|
|
@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
|
|||
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
|
||||
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
|
||||
* is running) */
|
||||
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||
|
||||
/* Write part of the status message */
|
||||
_status.flightPathAngleSp = flightPathAngleSp;
|
||||
|
|
|
|||
|
|
@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock
|
|||
{
|
||||
public:
|
||||
// methods
|
||||
BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
|
||||
BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) :
|
||||
SuperBlock(parent, name),
|
||||
_isAngularLimit(isAngularLimit),
|
||||
_isAngularLimit(fAngularLimit),
|
||||
_min(this, "MIN"),
|
||||
_max(this, "MAX")
|
||||
{};
|
||||
|
|
|
|||
|
|
@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req)
|
|||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemove(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
//auto hdr = req->header();
|
||||
|
||||
// for now, send error reply
|
||||
return kErrPerm;
|
||||
|
|
|
|||
|
|
@ -2238,7 +2238,7 @@ Mavlink::start(int argc, char *argv[])
|
|||
task_spawn_cmd(buf,
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1950,
|
||||
2700,
|
||||
(main_t)&Mavlink::start_helper,
|
||||
(const char **)argv);
|
||||
|
||||
|
|
|
|||
|
|
@ -396,32 +396,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
|
|||
void
|
||||
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_radio_status_t rstatus;
|
||||
mavlink_msg_radio_status_decode(msg, &rstatus);
|
||||
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
|
||||
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
|
||||
mavlink_radio_status_t rstatus;
|
||||
mavlink_msg_radio_status_decode(msg, &rstatus);
|
||||
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
|
||||
tstatus.rssi = rstatus.rssi;
|
||||
tstatus.remote_rssi = rstatus.remrssi;
|
||||
tstatus.txbuf = rstatus.txbuf;
|
||||
tstatus.noise = rstatus.noise;
|
||||
tstatus.remote_noise = rstatus.remnoise;
|
||||
tstatus.rxerrors = rstatus.rxerrors;
|
||||
tstatus.fixed = rstatus.fixed;
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
|
||||
tstatus.rssi = rstatus.rssi;
|
||||
tstatus.remote_rssi = rstatus.remrssi;
|
||||
tstatus.txbuf = rstatus.txbuf;
|
||||
tstatus.noise = rstatus.noise;
|
||||
tstatus.remote_noise = rstatus.remnoise;
|
||||
tstatus.rxerrors = rstatus.rxerrors;
|
||||
tstatus.fixed = rstatus.fixed;
|
||||
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
|
||||
} else {
|
||||
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
|
||||
}
|
||||
|
||||
/* this means that heartbeats alone won't be published to the radio status no more */
|
||||
_radio_status_available = true;
|
||||
}
|
||||
|
||||
/* this means that heartbeats alone won't be published to the radio status no more */
|
||||
_radio_status_available = true;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -452,28 +455,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||
void
|
||||
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_heartbeat_t hb;
|
||||
mavlink_msg_heartbeat_decode(msg, &hb);
|
||||
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
|
||||
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
|
||||
mavlink_heartbeat_t hb;
|
||||
mavlink_msg_heartbeat_decode(msg, &hb);
|
||||
|
||||
/* ignore own heartbeats, accept only heartbeats from GCS */
|
||||
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
|
||||
_telemetry_heartbeat_time = hrt_absolute_time();
|
||||
/* ignore own heartbeats, accept only heartbeats from GCS */
|
||||
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
|
||||
_telemetry_heartbeat_time = hrt_absolute_time();
|
||||
|
||||
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
|
||||
if (!_radio_status_available) {
|
||||
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
|
||||
if (!_radio_status_available) {
|
||||
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
|
||||
tstatus.timestamp = _telemetry_heartbeat_time;
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
tstatus.timestamp = _telemetry_heartbeat_time;
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
|
||||
} else {
|
||||
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -718,7 +724,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
|
||||
|
|
|
|||
|
|
@ -64,12 +64,15 @@
|
|||
#define rCCR REG(STM32_I2C_CCR_OFFSET)
|
||||
#define rTRISE REG(STM32_I2C_TRISE_OFFSET)
|
||||
|
||||
void i2c_reset(void);
|
||||
static int i2c_interrupt(int irq, void *context);
|
||||
static void i2c_rx_setup(void);
|
||||
static void i2c_tx_setup(void);
|
||||
static void i2c_rx_complete(void);
|
||||
static void i2c_tx_complete(void);
|
||||
#ifdef DEBUG
|
||||
static void i2c_dump(void);
|
||||
#endif
|
||||
|
||||
static DMA_HANDLE rx_dma;
|
||||
static DMA_HANDLE tx_dma;
|
||||
|
|
|
|||
|
|
@ -219,8 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values);
|
|||
extern void dsm_bind(uint16_t cmd, int pulses);
|
||||
extern int sbus_init(const char *device);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
|
||||
extern bool sbus1_output(uint16_t *values, uint16_t num_values);
|
||||
extern bool sbus2_output(uint16_t *values, uint16_t num_values);
|
||||
extern void sbus1_output(uint16_t *values, uint16_t num_values);
|
||||
extern void sbus2_output(uint16_t *values, uint16_t num_values);
|
||||
|
||||
/** global debug level for isr_debug() */
|
||||
extern volatile uint8_t debug_level;
|
||||
|
|
|
|||
|
|
@ -711,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
{
|
||||
#define SELECT_PAGE(_page_name) \
|
||||
do { \
|
||||
*values = &_page_name[0]; \
|
||||
*values = (uint16_t *)&_page_name[0]; \
|
||||
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
|
||||
} while(0)
|
||||
|
||||
|
|
|
|||
|
|
@ -116,14 +116,14 @@ sbus_init(const char *device)
|
|||
return sbus_fd;
|
||||
}
|
||||
|
||||
bool
|
||||
void
|
||||
sbus1_output(uint16_t *values, uint16_t num_values)
|
||||
{
|
||||
char a = 'A';
|
||||
write(sbus_fd, &a, 1);
|
||||
}
|
||||
|
||||
bool
|
||||
void
|
||||
sbus2_output(uint16_t *values, uint16_t num_values)
|
||||
{
|
||||
char b = 'B';
|
||||
|
|
|
|||
|
|
@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct log_GVSP_s log_GVSP;
|
||||
struct log_BATT_s log_BATT;
|
||||
struct log_DIST_s log_DIST;
|
||||
struct log_TELE_s log_TELE;
|
||||
struct log_TEL_s log_TEL;
|
||||
struct log_EST0_s log_EST0;
|
||||
struct log_EST1_s log_EST1;
|
||||
struct log_PWR_s log_PWR;
|
||||
|
|
@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
int esc_sub;
|
||||
int global_vel_sp_sub;
|
||||
int battery_sub;
|
||||
int telemetry_sub;
|
||||
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
int range_finder_sub;
|
||||
int estimator_status_sub;
|
||||
int tecs_status_sub;
|
||||
|
|
@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
|
||||
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
|
||||
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
|
||||
}
|
||||
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
|
||||
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
|
||||
|
|
@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* --- TELEMETRY --- */
|
||||
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
|
||||
log_msg.msg_type = LOG_TELE_MSG;
|
||||
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
|
||||
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
|
||||
log_msg.body.log_TELE.noise = buf.telemetry.noise;
|
||||
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
|
||||
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
|
||||
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
|
||||
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
|
||||
LOGBUFFER_WRITE_AND_COUNT(TELE);
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
|
||||
log_msg.msg_type = LOG_TEL0_MSG + i;
|
||||
log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
|
||||
log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
|
||||
log_msg.body.log_TEL.noise = buf.telemetry.noise;
|
||||
log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise;
|
||||
log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors;
|
||||
log_msg.body.log_TEL.fixed = buf.telemetry.fixed;
|
||||
log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf;
|
||||
log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time;
|
||||
LOGBUFFER_WRITE_AND_COUNT(TEL);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- BOTTOM DISTANCE --- */
|
||||
|
|
|
|||
|
|
@ -91,6 +91,14 @@ struct log_format_s {
|
|||
.labels = _labels \
|
||||
}
|
||||
|
||||
#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \
|
||||
.type = LOG_##_name##_MSG, \
|
||||
.length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \
|
||||
.name = #_name, \
|
||||
.format = _format, \
|
||||
.labels = _labels \
|
||||
}
|
||||
|
||||
#define LOG_FORMAT_MSG 0x80
|
||||
|
||||
#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
|
||||
|
|
|
|||
|
|
@ -276,18 +276,7 @@ struct log_DIST_s {
|
|||
uint8_t flags;
|
||||
};
|
||||
|
||||
/* --- TELE - TELEMETRY STATUS --- */
|
||||
#define LOG_TELE_MSG 22
|
||||
struct log_TELE_s {
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
// ID 22 available
|
||||
// ID 23 available
|
||||
|
||||
/* --- PWR - ONBOARD POWER SYSTEM --- */
|
||||
|
|
@ -385,6 +374,23 @@ struct log_EST1_s {
|
|||
float s[16];
|
||||
};
|
||||
|
||||
/* --- TEL0..3 - TELEMETRY STATUS --- */
|
||||
#define LOG_TEL0_MSG 34
|
||||
#define LOG_TEL1_MSG 35
|
||||
#define LOG_TEL2_MSG 36
|
||||
#define LOG_TEL3_MSG 37
|
||||
struct log_TEL_s {
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
uint64_t heartbeat_time;
|
||||
};
|
||||
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
|
|
@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
|
||||
LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
|
||||
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT
|
|||
|
||||
__EXPORT struct system_load_s system_load;
|
||||
|
||||
extern FAR struct _TCB *sched_gettcb(pid_t pid);
|
||||
extern FAR struct tcb_s *sched_gettcb(pid_t pid);
|
||||
|
||||
void cpuload_initialize_once()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -54,6 +54,9 @@
|
|||
|
||||
#include "systemlib.h"
|
||||
|
||||
// Didn't seem right to include up_internal.h, so direct extern instead.
|
||||
extern void up_systemreset(void) noreturn_function;
|
||||
|
||||
void
|
||||
systemreset(bool to_bootloader)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
|
|||
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
|
||||
|
||||
#include "topics/telemetry_status.h"
|
||||
ORB_DEFINE(telemetry_status, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
|
||||
|
||||
#include "topics/debug_key_value.h"
|
||||
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
|
||||
|
|
|
|||
|
|
@ -72,6 +72,18 @@ struct telemetry_status_s {
|
|||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(telemetry_status);
|
||||
ORB_DECLARE(telemetry_status_0);
|
||||
ORB_DECLARE(telemetry_status_1);
|
||||
ORB_DECLARE(telemetry_status_2);
|
||||
ORB_DECLARE(telemetry_status_3);
|
||||
|
||||
#define TELEMETRY_STATUS_ORB_ID_NUM 4
|
||||
|
||||
static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = {
|
||||
ORB_ID(telemetry_status_0),
|
||||
ORB_ID(telemetry_status_1),
|
||||
ORB_ID(telemetry_status_2),
|
||||
ORB_ID(telemetry_status_3),
|
||||
};
|
||||
|
||||
#endif /* TOPIC_TELEMETRY_STATUS_H */
|
||||
|
|
|
|||
|
|
@ -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. */
|
||||
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@
|
|||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <math.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
|
@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param)
|
|||
if (!(arg == NULL)) {
|
||||
|
||||
/* start search */
|
||||
char *ss = search_string;
|
||||
char *pp = p_name;
|
||||
const char *ss = search_string;
|
||||
const char *pp = p_name;
|
||||
bool mismatch = false;
|
||||
|
||||
/* XXX this comparison is only ok for trailing wildcards */
|
||||
|
|
|
|||
|
|
@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[])
|
|||
TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
|
||||
TEST_OP("Vector<3> length", v1.length());
|
||||
TEST_OP("Vector<3> length squared", v1.length_squared());
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
TEST_OP("Vector<3> element read", volatile float a = v1(0));
|
||||
TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
|
||||
#pragma GCC diagnostic pop
|
||||
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
|
||||
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue