forked from Archive/PX4-Autopilot
Merge branch 'master' into mavlinkrates2
This commit is contained in:
commit
17bfc06e9c
|
@ -157,6 +157,10 @@ private:
|
|||
perf_counter_t _pc_idle;
|
||||
perf_counter_t _pc_badidle;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
PX4IO_serial(PX4IO_serial &);
|
||||
PX4IO_serial& operator = (const PX4IO_serial &);
|
||||
|
||||
};
|
||||
|
||||
IOPacket PX4IO_serial::_dma_buffer;
|
||||
|
@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() :
|
|||
_tx_dma(nullptr),
|
||||
_rx_dma(nullptr),
|
||||
_rx_dma_status(_dma_status_inactive),
|
||||
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
|
||||
_bus_semaphore(SEM_INITIALIZER(0)),
|
||||
_completion_semaphore(SEM_INITIALIZER(0)),
|
||||
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
|
||||
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
|
||||
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
|
||||
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),
|
||||
|
|
|
@ -65,7 +65,8 @@
|
|||
|
||||
PX4IO_Uploader::PX4IO_Uploader() :
|
||||
_io_fd(-1),
|
||||
_fw_fd(-1)
|
||||
_fw_fd(-1),
|
||||
bl_rev(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
|||
}
|
||||
|
||||
int
|
||||
PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
|
||||
PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout)
|
||||
{
|
||||
struct pollfd fds[1];
|
||||
|
||||
|
@ -262,19 +263,19 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
|
|||
return -ETIMEDOUT;
|
||||
}
|
||||
|
||||
read(_io_fd, &c, 1);
|
||||
read(_io_fd, c, 1);
|
||||
#ifdef UDEBUG
|
||||
log("recv 0x%02x", c);
|
||||
log("recv_bytes 0x%02x", c);
|
||||
#endif
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_Uploader::recv(uint8_t *p, unsigned count)
|
||||
PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
|
||||
{
|
||||
int ret;
|
||||
int ret = OK;
|
||||
while (count--) {
|
||||
ret = recv(*p++, 5000);
|
||||
ret = recv_byte_with_timeout(p++, 5000);
|
||||
|
||||
if (ret != OK)
|
||||
break;
|
||||
|
@ -289,10 +290,10 @@ PX4IO_Uploader::drain()
|
|||
int ret;
|
||||
|
||||
do {
|
||||
// the small recv timeout here is to allow for fast
|
||||
// the small recv_bytes timeout here is to allow for fast
|
||||
// drain when rebooting the io board for a forced
|
||||
// update of the fw without using the safety switch
|
||||
ret = recv(c, 40);
|
||||
ret = recv_byte_with_timeout(&c, 40);
|
||||
|
||||
#ifdef UDEBUG
|
||||
if (ret == OK) {
|
||||
|
@ -331,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout)
|
|||
uint8_t c[2];
|
||||
int ret;
|
||||
|
||||
ret = recv(c[0], timeout);
|
||||
ret = recv_byte_with_timeout(c, timeout);
|
||||
|
||||
if (ret != OK)
|
||||
return ret;
|
||||
|
||||
ret = recv(c[1], timeout);
|
||||
ret = recv_byte_with_timeout(c + 1, timeout);
|
||||
|
||||
if (ret != OK)
|
||||
return ret;
|
||||
|
@ -372,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
|
|||
send(param);
|
||||
send(PROTO_EOC);
|
||||
|
||||
ret = recv((uint8_t *)&val, sizeof(val));
|
||||
ret = recv_bytes((uint8_t *)&val, sizeof(val));
|
||||
|
||||
if (ret != OK)
|
||||
return ret;
|
||||
|
@ -513,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
|
|||
for (ssize_t i = 0; i < count; i++) {
|
||||
uint8_t c;
|
||||
|
||||
ret = recv(c, 5000);
|
||||
ret = recv_byte_with_timeout(&c, 5000);
|
||||
|
||||
if (ret != OK) {
|
||||
log("%d: got %d waiting for bytes", sent + i, ret);
|
||||
|
@ -600,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
|
|||
send(PROTO_GET_CRC);
|
||||
send(PROTO_EOC);
|
||||
|
||||
ret = recv((uint8_t*)(&crc), sizeof(crc));
|
||||
ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
|
||||
|
||||
if (ret != OK) {
|
||||
log("did not receive CRC checksum");
|
||||
|
|
|
@ -74,19 +74,19 @@ private:
|
|||
INFO_BOARD_REV = 3, /**< board revision */
|
||||
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
|
||||
|
||||
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
|
||||
PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */
|
||||
|
||||
};
|
||||
|
||||
int _io_fd;
|
||||
int _fw_fd;
|
||||
|
||||
uint32_t bl_rev; /**< bootloader revision */
|
||||
uint32_t bl_rev; /**< bootloader revision */
|
||||
|
||||
void log(const char *fmt, ...);
|
||||
|
||||
int recv(uint8_t &c, unsigned timeout);
|
||||
int recv(uint8_t *p, unsigned count);
|
||||
int recv_byte_with_timeout(uint8_t *c, unsigned timeout);
|
||||
int recv_bytes(uint8_t *p, unsigned count);
|
||||
void drain();
|
||||
int send(uint8_t c);
|
||||
int send(uint8_t *p, unsigned count);
|
||||
|
|
|
@ -383,7 +383,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
|
|||
Mavlink *inst;
|
||||
LL_FOREACH(_mavlink_instances, inst) {
|
||||
if (inst != self) {
|
||||
inst->pass_message(msg);
|
||||
|
||||
/* if not in normal mode, we are an onboard link
|
||||
* onboard links should only pass on messages from the same system ID */
|
||||
if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
|
||||
inst->pass_message(msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -241,7 +241,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||
} else {
|
||||
|
||||
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
|
||||
warnx("ignoring CMD spoofed with same SYS/COMP ID");
|
||||
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
|
||||
mavlink_system.sysid, mavlink_system.compid);
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
|
@ -314,30 +314,36 @@ Mission::set_mission_items()
|
|||
/* set previous position setpoint to current */
|
||||
set_previous_pos_setpoint();
|
||||
|
||||
/* get home distance state */
|
||||
bool home_dist_ok = check_dist_1wp();
|
||||
/* the home dist check provides user feedback, so we initialize it to this */
|
||||
bool user_feedback_done = !home_dist_ok;
|
||||
|
||||
/* try setting onboard mission item */
|
||||
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_ONBOARD) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_ONBOARD;
|
||||
|
||||
/* try setting offboard mission item */
|
||||
} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
|
||||
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_OFFBOARD) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
|
||||
} else {
|
||||
/* no mission available, switch to loiter */
|
||||
/* no mission available or mission finished, switch to loiter */
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: mission finished");
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: no mission available");
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
|
||||
} else if (!user_feedback_done) {
|
||||
/* only tell users that we got no mission if there has not been any
|
||||
* better, more specific feedback yet
|
||||
*/
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_NONE;
|
||||
|
||||
|
@ -397,7 +403,7 @@ Mission::set_mission_items()
|
|||
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
|
||||
}
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
|
||||
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
|
@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
|
|||
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"#audio: ERROR waypoint could not be read");
|
||||
"ERROR waypoint could not be read");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
|
|||
/* not supposed to happen unless the datamanager can't access the
|
||||
* dataman */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"#audio: ERROR DO JUMP waypoint could not be written");
|
||||
"ERROR DO JUMP waypoint could not be written");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
|
|||
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
|
||||
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: DO JUMP repetitions completed");
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"DO JUMP repetitions completed");
|
||||
/* no more DO_JUMPS, therefore just try to continue with next mission item */
|
||||
(*mission_index_ptr)++;
|
||||
}
|
||||
|
@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
|
|||
|
||||
/* we have given up, we don't want to cycle forever */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"#audio: ERROR DO JUMP is cycling, giving up");
|
||||
"ERROR DO JUMP is cycling, giving up");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -38,4 +38,4 @@
|
|||
MODULE_COMMAND = nshterm
|
||||
SRCS = nshterm.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
MODULE_STACKSIZE = 1400
|
||||
|
|
Loading…
Reference in New Issue