zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Mavlink: get orb_listener to work

sbg
Julian Oes 11 years ago
parent
19105bebc5
commit
ea2a69d8bf
4 changed files with 66 additions and 45 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      src/modules/mavlink/mavlink_main.cpp
  2. 74
      src/modules/mavlink/mavlink_orb_listener.cpp
  3. 6
      src/modules/mavlink/mavlink_orb_listener.h
  4. 2
      src/modules/uORB/topics/vehicle_attitude.h

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

@ -153,6 +153,7 @@ namespace mavlink @@ -153,6 +153,7 @@ namespace mavlink
Mavlink::Mavlink() :
// _mavlink_fd(-1),
_next(nullptr),
_task_should_exit(false),
thread_running(false),
_mavlink_task(-1),
@ -1448,6 +1449,8 @@ Mavlink::task_main(int argc, char *argv[]) @@ -1448,6 +1449,8 @@ Mavlink::task_main(int argc, char *argv[])
_baudrate = 57600;
_chan = MAVLINK_COMM_0;
_mode = MODE_OFFBOARD;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
@ -1488,7 +1491,23 @@ Mavlink::task_main(int argc, char *argv[]) @@ -1488,7 +1491,23 @@ Mavlink::task_main(int argc, char *argv[])
warnx("MAVLink v1.0 serial interface starting...");
/* 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 */
fflush(stdout);
@ -1506,12 +1525,12 @@ Mavlink::task_main(int argc, char *argv[]) @@ -1506,12 +1525,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
/* start the MAVLink receiver */
// MavlinkReceiver rcv(this);
//receive_thread = MavlinkReceiver::receive_start(this);
// MavlinkReceiver rcv(this);
receive_thread = MavlinkReceiver::receive_start(this);
/* start the ORB receiver */
//MavlinkOrbListener listener(this);
//uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
// MavlinkOrbListener listener(this);
uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
/* initialize waypoint manager */
mavlink_wpm_init(wpm);

74
src/modules/mavlink/mavlink_orb_listener.cpp
Unescape View File

@ -36,6 +36,7 @@ @@ -36,6 +36,7 @@
* Monitors ORB topics and sends update messages as appropriate.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
// XXX trim includes
@ -80,7 +81,7 @@ cm_uint16_from_m_float(float m) @@ -80,7 +81,7 @@ cm_uint16_from_m_float(float m)
return (uint16_t)(m * 100.0f);
}
MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
thread_should_exit(false),
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
@ -88,31 +89,31 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : @@ -88,31 +89,31 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
_listeners(nullptr),
_n_listeners(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_gps_position, &_mavlink->get_subs()->gps_sub, 0);
add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0);
add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0);
add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0);
add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0);
add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0);
add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0);
add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0);
add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2);
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3);
add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0);
add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0);
add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0);
add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0);
add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0);
add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0);
add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0);
add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0);
add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0);
add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_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_gps_position, &(_mavlink->get_subs()->gps_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0);
add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0);
add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0);
add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0);
add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0);
add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0);
add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0);
add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0);
add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0);
add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0);
add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1);
add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2);
add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3);
add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0);
add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0);
add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0);
add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0);
add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0);
add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_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) @@ -124,15 +125,18 @@ void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l)
nl->subp = subp;
nl->arg = arg;
nl->next = nullptr;
// Register it
struct listener *next = _listeners;
while (next->next != nullptr) {
next = next->next;
nl->mavlink = _mavlink;
nl->listener = this;
if (_listeners == nullptr) {
_listeners = nl;
} else {
struct listener *next = _listeners;
while (next->next != nullptr) {
next = next->next;
}
next->next = nl;
}
// Attach
next->next = nl;
_n_listeners++;
}
@ -271,7 +275,7 @@ void @@ -271,7 +275,7 @@ void
MavlinkOrbListener::l_vehicle_status(const struct listener *l)
{
/* 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);
/* enable or disable HIL */

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

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

2
src/modules/uORB/topics/vehicle_attitude.h
Unescape View File

@ -76,7 +76,7 @@ struct vehicle_attitude_s { @@ -76,7 +76,7 @@ struct vehicle_attitude_s {
float rate_offsets[3]; /**< Offsets of the body angular rates from zero */
float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
float q[4]; /**< Quaternion (NED) */
float g_comp[3]; /**< Compensated gravity vector */
float g_comp[3]; /**< Compensated gravity vector */
bool R_valid; /**< Rotation matrix valid */
bool q_valid; /**< Quaternion valid */

Write Preview
Loading…
Cancel
Save