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

mavlink: more streams implemented, stack size returned to 2048

This commit is contained in:
Anton Babushkin 2014-02-26 22:47:19 +04:00
parent f9619e3934
commit 85dc422d98
2 changed files with 164 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -61,14 +61,6 @@
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -79,6 +71,10 @@
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "mavlink_bridge_header.h"
#include "mavlink_main.h"
#include "mavlink_messages.h"
@ -1542,13 +1538,11 @@ Mavlink::task_main(int argc, char *argv[])
add_stream("HEARTBEAT", 1.0f);
add_stream("SYS_STATUS", 1.0f);
add_stream("HIGHRES_IMU", 20.0f);
// add_stream("RAW_IMU", 10.0f);
add_stream("ATTITUDE", 20.0f);
// add_stream("NAMED_VALUE_FLOAT", 5.0f);
// add_stream("SERVO_OUTPUT_RAW", 2.0f);
// add_stream("GPS_RAW_INT", 2.0f);
// add_stream("MANUAL_CONTROL", 2.0f);
add_stream("HIGHRES_IMU", 1.0f);
add_stream("ATTITUDE", 10.0f);
add_stream("GPS_RAW_INT", 1.0f);
add_stream("GLOBAL_POSITION_INT", 5.0f);
add_stream("LOCAL_POSITION_NED", 5.0f);
while (!_task_should_exit) {
/* main loop */
@ -1656,7 +1650,7 @@ Mavlink::start(int argc, char *argv[])
/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
2048,
(main_t)&Mavlink::start_helper,
(const char **)argv);

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

@ -6,12 +6,32 @@
*/
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_combined.h>
#include "mavlink_messages.h"
uint16_t
cm_uint16_from_m_float(float m)
{
if (m < 0.0f) {
return 0;
} else if (m > 655.35f) {
return 65535;
}
return (uint16_t)(m * 100.0f);
}
class MavlinkStreamHeartbeat : public MavlinkStream {
public:
const char *get_name()
@ -279,11 +299,145 @@ protected:
};
class MavlinkStreamGPSRawInt : public MavlinkStream {
public:
const char *get_name()
{
return "GPS_RAW_INT";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamGPSRawInt();
}
private:
MavlinkOrbSubscription *gps_sub;
struct vehicle_gps_position_s *gps;
protected:
void subscribe(Mavlink *mavlink)
{
gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s));
gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
}
void send(const hrt_abstime t) {
gps_sub->update(t);
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
gps->lat,
gps->lon,
gps->alt,
cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible);
}
};
class MavlinkStreamGlobalPositionInt : public MavlinkStream {
public:
const char *get_name()
{
return "GLOBAL_POSITION_INT";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionInt();
}
private:
MavlinkOrbSubscription *pos_sub;
MavlinkOrbSubscription *home_sub;
struct vehicle_global_position_s *pos;
struct home_position_s *home;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s));
pos = (struct vehicle_global_position_s *)pos_sub->get_data();
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t) {
pos_sub->update(t);
home_sub->update(t);
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
};
class MavlinkStreamLocalPositionNED : public MavlinkStream {
public:
const char *get_name()
{
return "LOCAL_POSITION_NED";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionNED();
}
private:
MavlinkOrbSubscription *pos_sub;
struct vehicle_local_position_s *pos;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s));
pos = (struct vehicle_local_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t) {
pos_sub->update(t);
mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->vx,
pos->vy,
pos->vz);
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
new MavlinkStreamHighresIMU(),
new MavlinkStreamAttitude(),
new MavlinkStreamGPSRawInt(),
new MavlinkStreamGlobalPositionInt(),
new MavlinkStreamLocalPositionNED(),
nullptr
};
@ -347,22 +501,6 @@ MavlinkStream *streams_list[] = {
// /* copy gps data into local buffer */
// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps);
//
// /* GPS COG is 0..2PI in degrees * 1e2 */
// float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
//
// /* GPS position */
// mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(),
// gps.timestamp_position,
// gps.fix_type,
// gps.lat,
// gps.lon,
// gps.alt,
// cm_uint16_from_m_float(gps.eph_m),
// cm_uint16_from_m_float(gps.epv_m),
// gps.vel_m_s * 1e2f, // from m/s to cm/s
// cog_deg * 1e2f, // from deg to deg * 100
// gps.satellites_visible);
//
// /* update SAT info every 10 seconds */
// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) {
// mavlink_msg_gps_status_send(l->mavlink->get_chan(),
@ -414,40 +552,6 @@ MavlinkStream *streams_list[] = {
// }
//}
//
//void
//MavlinkOrbListener::l_global_position(const struct listener *l)
//{
// /* copy global position data into local buffer */
// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos);
//
// mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
// l->listener->global_pos.timestamp / 1000,
// l->listener->global_pos.lat * 1e7,
// l->listener->global_pos.lon * 1e7,
// l->listener->global_pos.alt * 1000.0f,
// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f,
// l->listener->global_pos.vel_n * 100.0f,
// l->listener->global_pos.vel_e * 100.0f,
// l->listener->global_pos.vel_d * 100.0f,
// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
//}
//
//void
//MavlinkOrbListener::l_local_position(const struct listener *l)
//{
// /* copy local position data into local buffer */
// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos);
//
// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
// l->listener->local_pos.timestamp / 1000,
// l->listener->local_pos.x,
// l->listener->local_pos.y,
// l->listener->local_pos.z,
// l->listener->local_pos.vx,
// l->listener->local_pos.vy,
// l->listener->local_pos.vz);
//}
//
//void
//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)