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

Merge branch 'sensor_load' into cmake-2

This commit is contained in:
Lorenz Meier 2015-09-27 18:10:18 +02:00
parent ec0f53f7c1 afb68cb0ab
commit 96e0ac4581
3 changed files with 40 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mavlink/mavlink_main.cpp
View File

@ -1659,7 +1659,7 @@ Mavlink::task_main(int argc, char *argv[])
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("HIGHRES_IMU", 2.0f);
configure_stream("ATTITUDE", 20.0f);
configure_stream("VFR_HUD", 8.0f);
@ -1683,6 +1683,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("ATTITUDE_TARGET", 10.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
@ -1705,6 +1706,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VFR_HUD", 5.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 10.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("ATTITUDE_TARGET", 10.0f);
configure_stream("BATTERY_STATUS", 1.0f);
configure_stream("SYSTEM_TIME", 1.0f);
@ -1716,7 +1718,7 @@ Mavlink::task_main(int argc, char *argv[])
case MAVLINK_MODE_CONFIG:
// Enable a number of interesting streams we want via USB
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("GLOBAL_POSITION_INT", 10.0f);
configure_stream("ATTITUDE_TARGET", 8.0f);
configure_stream("PARAM_VALUE", 300.0f);

37
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1326,43 +1326,43 @@ protected:
};
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
class MavlinkStreamHomePosition : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamGPSGlobalOrigin::get_name_static();
return MavlinkStreamHomePosition::get_name_static();
}
static const char *get_name_static()
{
return "GPS_GLOBAL_ORIGIN";
return "HOME_POSITION";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
return MAVLINK_MSG_ID_HOME_POSITION;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamGPSGlobalOrigin(mavlink);
return new MavlinkStreamHomePosition(mavlink);
}
unsigned get_size()
{
return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _home_sub->is_published() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_home_sub;
/* do not allow top copying this class */
MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &);
MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &);
MavlinkStreamHomePosition(MavlinkStreamHomePosition &);
MavlinkStreamHomePosition& operator = (const MavlinkStreamHomePosition &);
protected:
explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink),
explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink),
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
{}
@ -1374,13 +1374,26 @@ protected:
struct home_position_s home;
if (_home_sub->update(&home)) {
mavlink_gps_global_origin_t msg;
mavlink_home_position_t msg;
msg.latitude = home.lat * 1e7;
msg.longitude = home.lon * 1e7;
msg.altitude = home.alt * 1e3f;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg);
msg.x = home.x;
msg.y = home.y;
msg.z = home.z;
msg.q[0] = 1.0f;
msg.q[1] = 0.0f;
msg.q[2] = 0.0f;
msg.q[3] = 0.0f;
msg.approach_x = 0.0f;
msg.approach_y = 0.0f;
msg.approach_z = 0.0f;
_mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg);
}
}
}
@ -2437,7 +2450,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),

11
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -307,6 +307,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
/* send autopilot version message */
_mavlink->send_autopilot_capabilites();
} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
@ -363,6 +367,13 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
/* terminate other threads and this thread */
_mavlink->_task_should_exit = true;
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
/* send autopilot version message */
_mavlink->send_autopilot_capabilites();
} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {