|
|
|
@ -91,32 +91,32 @@ static uint64_t last_sensor_timestamp;
@@ -91,32 +91,32 @@ static uint64_t last_sensor_timestamp;
|
|
|
|
|
static void *uorb_receive_thread(void *arg); |
|
|
|
|
|
|
|
|
|
struct listener { |
|
|
|
|
void (*callback)(struct listener *l); |
|
|
|
|
void (*callback)(const struct listener *l); |
|
|
|
|
int *subp; |
|
|
|
|
uintptr_t arg; |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
static void l_sensor_combined(struct listener *l); |
|
|
|
|
static void l_vehicle_attitude(struct listener *l); |
|
|
|
|
static void l_vehicle_gps_position(struct listener *l); |
|
|
|
|
static void l_vehicle_status(struct listener *l); |
|
|
|
|
static void l_rc_channels(struct listener *l); |
|
|
|
|
static void l_input_rc(struct listener *l); |
|
|
|
|
static void l_global_position(struct listener *l); |
|
|
|
|
static void l_local_position(struct listener *l); |
|
|
|
|
static void l_global_position_setpoint(struct listener *l); |
|
|
|
|
static void l_local_position_setpoint(struct listener *l); |
|
|
|
|
static void l_attitude_setpoint(struct listener *l); |
|
|
|
|
static void l_actuator_outputs(struct listener *l); |
|
|
|
|
static void l_actuator_armed(struct listener *l); |
|
|
|
|
static void l_manual_control_setpoint(struct listener *l); |
|
|
|
|
static void l_vehicle_attitude_controls(struct listener *l); |
|
|
|
|
static void l_debug_key_value(struct listener *l); |
|
|
|
|
static void l_optical_flow(struct listener *l); |
|
|
|
|
static void l_vehicle_rates_setpoint(struct listener *l); |
|
|
|
|
static void l_home(struct listener *l); |
|
|
|
|
|
|
|
|
|
struct listener listeners[] = { |
|
|
|
|
static void l_sensor_combined(const struct listener *l); |
|
|
|
|
static void l_vehicle_attitude(const struct listener *l); |
|
|
|
|
static void l_vehicle_gps_position(const struct listener *l); |
|
|
|
|
static void l_vehicle_status(const struct listener *l); |
|
|
|
|
static void l_rc_channels(const struct listener *l); |
|
|
|
|
static void l_input_rc(const struct listener *l); |
|
|
|
|
static void l_global_position(const struct listener *l); |
|
|
|
|
static void l_local_position(const struct listener *l); |
|
|
|
|
static void l_global_position_setpoint(const struct listener *l); |
|
|
|
|
static void l_local_position_setpoint(const struct listener *l); |
|
|
|
|
static void l_attitude_setpoint(const struct listener *l); |
|
|
|
|
static void l_actuator_outputs(const struct listener *l); |
|
|
|
|
static void l_actuator_armed(const struct listener *l); |
|
|
|
|
static void l_manual_control_setpoint(const struct listener *l); |
|
|
|
|
static void l_vehicle_attitude_controls(const struct listener *l); |
|
|
|
|
static void l_debug_key_value(const struct listener *l); |
|
|
|
|
static void l_optical_flow(const struct listener *l); |
|
|
|
|
static void l_vehicle_rates_setpoint(const struct listener *l); |
|
|
|
|
static void l_home(const struct listener *l); |
|
|
|
|
|
|
|
|
|
static const struct listener listeners[] = { |
|
|
|
|
{l_sensor_combined, &mavlink_subs.sensor_sub, 0}, |
|
|
|
|
{l_vehicle_attitude, &mavlink_subs.att_sub, 0}, |
|
|
|
|
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, |
|
|
|
@ -144,7 +144,7 @@ struct listener listeners[] = {
@@ -144,7 +144,7 @@ struct listener listeners[] = {
|
|
|
|
|
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_sensor_combined(struct listener *l) |
|
|
|
|
l_sensor_combined(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct sensor_combined_s raw; |
|
|
|
|
|
|
|
|
@ -199,7 +199,7 @@ l_sensor_combined(struct listener *l)
@@ -199,7 +199,7 @@ l_sensor_combined(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_vehicle_attitude(struct listener *l) |
|
|
|
|
l_vehicle_attitude(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct vehicle_attitude_s att; |
|
|
|
|
|
|
|
|
@ -222,7 +222,7 @@ l_vehicle_attitude(struct listener *l)
@@ -222,7 +222,7 @@ l_vehicle_attitude(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_vehicle_gps_position(struct listener *l) |
|
|
|
|
l_vehicle_gps_position(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct vehicle_gps_position_s gps; |
|
|
|
|
|
|
|
|
@ -256,7 +256,7 @@ l_vehicle_gps_position(struct listener *l)
@@ -256,7 +256,7 @@ l_vehicle_gps_position(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_vehicle_status(struct listener *l) |
|
|
|
|
l_vehicle_status(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
/* immediately communicate state changes back to user */ |
|
|
|
|
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); |
|
|
|
@ -280,7 +280,7 @@ l_vehicle_status(struct listener *l)
@@ -280,7 +280,7 @@ l_vehicle_status(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_rc_channels(struct listener *l) |
|
|
|
|
l_rc_channels(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
/* copy rc channels into local buffer */ |
|
|
|
|
orb_copy(ORB_ID(rc_channels), rc_sub, &rc); |
|
|
|
@ -288,7 +288,7 @@ l_rc_channels(struct listener *l)
@@ -288,7 +288,7 @@ l_rc_channels(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_input_rc(struct listener *l) |
|
|
|
|
l_input_rc(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
/* copy rc channels into local buffer */ |
|
|
|
|
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); |
|
|
|
@ -310,7 +310,7 @@ l_input_rc(struct listener *l)
@@ -310,7 +310,7 @@ l_input_rc(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_global_position(struct listener *l) |
|
|
|
|
l_global_position(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
/* copy global position data into local buffer */ |
|
|
|
|
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); |
|
|
|
@ -340,7 +340,7 @@ l_global_position(struct listener *l)
@@ -340,7 +340,7 @@ l_global_position(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_local_position(struct listener *l) |
|
|
|
|
l_local_position(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
/* copy local position data into local buffer */ |
|
|
|
|
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); |
|
|
|
@ -357,7 +357,7 @@ l_local_position(struct listener *l)
@@ -357,7 +357,7 @@ l_local_position(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_global_position_setpoint(struct listener *l) |
|
|
|
|
l_global_position_setpoint(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct vehicle_global_position_setpoint_s global_sp; |
|
|
|
|
|
|
|
|
@ -379,7 +379,7 @@ l_global_position_setpoint(struct listener *l)
@@ -379,7 +379,7 @@ l_global_position_setpoint(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_local_position_setpoint(struct listener *l) |
|
|
|
|
l_local_position_setpoint(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct vehicle_local_position_setpoint_s local_sp; |
|
|
|
|
|
|
|
|
@ -396,7 +396,7 @@ l_local_position_setpoint(struct listener *l)
@@ -396,7 +396,7 @@ l_local_position_setpoint(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_attitude_setpoint(struct listener *l) |
|
|
|
|
l_attitude_setpoint(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct vehicle_attitude_setpoint_s att_sp; |
|
|
|
|
|
|
|
|
@ -413,7 +413,7 @@ l_attitude_setpoint(struct listener *l)
@@ -413,7 +413,7 @@ l_attitude_setpoint(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_vehicle_rates_setpoint(struct listener *l) |
|
|
|
|
l_vehicle_rates_setpoint(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct vehicle_rates_setpoint_s rates_sp; |
|
|
|
|
|
|
|
|
@ -430,7 +430,7 @@ l_vehicle_rates_setpoint(struct listener *l)
@@ -430,7 +430,7 @@ l_vehicle_rates_setpoint(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_actuator_outputs(struct listener *l) |
|
|
|
|
l_actuator_outputs(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct actuator_outputs_s act_outputs; |
|
|
|
|
|
|
|
|
@ -529,13 +529,13 @@ l_actuator_outputs(struct listener *l)
@@ -529,13 +529,13 @@ l_actuator_outputs(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_actuator_armed(struct listener *l) |
|
|
|
|
l_actuator_armed(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_manual_control_setpoint(struct listener *l) |
|
|
|
|
l_manual_control_setpoint(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct manual_control_setpoint_s man_control; |
|
|
|
|
|
|
|
|
@ -553,7 +553,7 @@ l_manual_control_setpoint(struct listener *l)
@@ -553,7 +553,7 @@ l_manual_control_setpoint(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_vehicle_attitude_controls(struct listener *l) |
|
|
|
|
l_vehicle_attitude_controls(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct actuator_controls_effective_s actuators; |
|
|
|
|
|
|
|
|
@ -581,7 +581,7 @@ l_vehicle_attitude_controls(struct listener *l)
@@ -581,7 +581,7 @@ l_vehicle_attitude_controls(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_debug_key_value(struct listener *l) |
|
|
|
|
l_debug_key_value(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct debug_key_value_s debug; |
|
|
|
|
|
|
|
|
@ -597,7 +597,7 @@ l_debug_key_value(struct listener *l)
@@ -597,7 +597,7 @@ l_debug_key_value(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_optical_flow(struct listener *l) |
|
|
|
|
l_optical_flow(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct optical_flow_s flow; |
|
|
|
|
|
|
|
|
@ -608,7 +608,7 @@ l_optical_flow(struct listener *l)
@@ -608,7 +608,7 @@ l_optical_flow(struct listener *l)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
l_home(struct listener *l) |
|
|
|
|
l_home(const struct listener *l) |
|
|
|
|
{ |
|
|
|
|
struct home_position_s home; |
|
|
|
|
|
|
|
|
|