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

Mavlink: get orb_listener to work

This commit is contained in:
Julian Oes 2014-02-11 22:36:36 +01:00
parent 19105bebc5
commit ea2a69d8bf
4 changed files with 65 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -153,6 +153,7 @@ namespace mavlink
Mavlink::Mavlink() : Mavlink::Mavlink() :
// _mavlink_fd(-1), // _mavlink_fd(-1),
_next(nullptr),
_task_should_exit(false), _task_should_exit(false),
thread_running(false), thread_running(false),
_mavlink_task(-1), _mavlink_task(-1),
@ -1448,6 +1449,8 @@ Mavlink::task_main(int argc, char *argv[])
_baudrate = 57600; _baudrate = 57600;
_chan = MAVLINK_COMM_0; _chan = MAVLINK_COMM_0;
_mode = MODE_OFFBOARD;
/* work around some stupidity in task_create's argv handling */ /* work around some stupidity in task_create's argv handling */
argc -= 2; argc -= 2;
argv += 2; argv += 2;
@ -1488,7 +1491,23 @@ Mavlink::task_main(int argc, char *argv[])
warnx("MAVLink v1.0 serial interface starting..."); warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */ /* inform about mode */
warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); switch (_mode) {
case MODE_TX_HEARTBEAT_ONLY:
warnx("MODE_TX_HEARTBEAT_ONLY");
break;
case MODE_OFFBOARD:
warnx("MODE_OFFBOARD");
break;
case MODE_ONBOARD:
warnx("MODE_ONBOARD");
break;
case MODE_HIL:
warnx("MODE_HIL");
break;
default:
warnx("Error: Unknown mode");
break;
}
/* Flush stdout in case MAVLink is about to take it over */ /* Flush stdout in case MAVLink is about to take it over */
fflush(stdout); fflush(stdout);
@ -1506,12 +1525,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system(); mavlink_update_system();
/* start the MAVLink receiver */ /* start the MAVLink receiver */
// MavlinkReceiver rcv(this); // MavlinkReceiver rcv(this);
//receive_thread = MavlinkReceiver::receive_start(this); receive_thread = MavlinkReceiver::receive_start(this);
/* start the ORB receiver */ /* start the ORB receiver */
//MavlinkOrbListener listener(this); // MavlinkOrbListener listener(this);
//uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
/* initialize waypoint manager */ /* initialize waypoint manager */
mavlink_wpm_init(wpm); mavlink_wpm_init(wpm);

62
src/modules/mavlink/mavlink_orb_listener.cpp
View File

@ -36,6 +36,7 @@
* Monitors ORB topics and sends update messages as appropriate. * Monitors ORB topics and sends update messages as appropriate.
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/ */
// XXX trim includes // XXX trim includes
@ -88,31 +89,31 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
_listeners(nullptr), _listeners(nullptr),
_n_listeners(0) _n_listeners(0)
{ {
add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0); add_listener(MavlinkOrbListener::l_sensor_combined, &(_mavlink->get_subs()->sensor_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0); add_listener(MavlinkOrbListener::l_vehicle_attitude, &(_mavlink->get_subs()->att_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0); add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0); add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0);
add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0); add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0);
add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0); add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0);
add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0); add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0);
add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0); add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0);
add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0); add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0);
add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0); add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0);
add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0); add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0); add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1); add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2); add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3); add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1);
add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0); add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2);
add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0); add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3);
add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0); add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0);
add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0); add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0);
add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0); add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0); add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0);
add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0); add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0);
add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0); add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0);
add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0); add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_sub), 0);
add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0); add_listener(MavlinkOrbListener::l_nav_cap, &(_mavlink->get_subs()->navigation_capabilities_sub), 0);
} }
@ -124,15 +125,18 @@ void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l)
nl->subp = subp; nl->subp = subp;
nl->arg = arg; nl->arg = arg;
nl->next = nullptr; nl->next = nullptr;
nl->mavlink = _mavlink;
nl->listener = this;
// Register it if (_listeners == nullptr) {
_listeners = nl;
} else {
struct listener *next = _listeners; struct listener *next = _listeners;
while (next->next != nullptr) { while (next->next != nullptr) {
next = next->next; next = next->next;
} }
// Attach
next->next = nl; next->next = nl;
}
_n_listeners++; _n_listeners++;
} }
@ -271,7 +275,7 @@ void
MavlinkOrbListener::l_vehicle_status(const struct listener *l) MavlinkOrbListener::l_vehicle_status(const struct listener *l)
{ {
/* immediately communicate state _mavlink->get_chan()ges back to user */ /* immediately communicate state _mavlink->get_chan()ges back to user */
orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status); orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status));
orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
/* enable or disable HIL */ /* enable or disable HIL */

6
src/modules/mavlink/mavlink_orb_listener.h
View File

@ -36,6 +36,7 @@
* MAVLink 1.0 protocol interface definition. * MAVLink 1.0 protocol interface definition.
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/ */
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
@ -106,7 +107,7 @@ public:
void (*callback)(const struct listener *l); void (*callback)(const struct listener *l);
int *subp; int *subp;
uintptr_t arg; uintptr_t arg;
struct listener* next; struct listener *next;
Mavlink *mavlink; Mavlink *mavlink;
MavlinkOrbListener* listener; MavlinkOrbListener* listener;
}; };
@ -172,9 +173,6 @@ private:
struct airspeed_s airspeed; struct airspeed_s airspeed;
struct home_position_s home; struct home_position_s home;
int status_sub;
int rc_sub;
unsigned int sensors_raw_counter; unsigned int sensors_raw_counter;
unsigned int attitude_counter; unsigned int attitude_counter;
unsigned int gps_counter; unsigned int gps_counter;