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

Merged rates setpoints

sbg
Lorenz Meier 12 years ago
parent
2f3f0aec46 98657b8ae4
commit
c08135ffef
3 changed files with 25 additions and 78 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 78
      apps/mavlink/mavlink_parameters.c
  2. 23
      apps/mavlink/orb_listener.c
  3. 2
      apps/mavlink/orb_topics.h

78
apps/mavlink/mavlink_parameters.c
Unescape View File

@ -72,20 +72,6 @@ static unsigned int mavlink_param_queue_index = 0;
*/ */
void mavlink_pm_callback(void *arg, param_t param); void mavlink_pm_callback(void *arg, param_t param);
/**
* Save parameters to EEPROM.
*
* Stores the parameters to /eeprom/parameters
*/
static int mavlink_pm_save_eeprom(void);
/**
* Load parameters from EEPROM.
*
* Loads the parameters from /eeprom/parameters
*/
static int mavlink_pm_load_eeprom(void);
void mavlink_pm_callback(void *arg, param_t param) void mavlink_pm_callback(void *arg, param_t param)
{ {
mavlink_pm_send_param(param); mavlink_pm_send_param(param);
@ -232,69 +218,5 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
} }
} break; } break;
// case MAVLINK_MSG_ID_COMMAND_LONG: {
// mavlink_command_long_t cmd_mavlink;
// mavlink_msg_command_long_decode(msg, &cmd_mavlink);
// uint8_t result = MAV_RESULT_UNSUPPORTED;
// if (cmd_mavlink.target_system == mavlink_system.sysid &&
// ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
// // XXX move this to LOW PRIO THREAD of commander app
// /* preflight parameter load / store */
// if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
// /* Read all parameters from EEPROM to RAM */
// if (((int)(cmd_mavlink.param1)) == 0) {
// /* read all parameters from EEPROM to RAM */
// int read_ret = param_load_default();
// if (read_ret == OK) {
// //printf("[mavlink pm] Loaded EEPROM params in RAM\n");
// mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file());
// result = MAV_RESULT_ACCEPTED;
// } else if (read_ret == 1) {
// mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file());
// result = MAV_RESULT_ACCEPTED;
// } else {
// if (read_ret < -1) {
// mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file());
// } else {
// mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file());
// }
// result = MAV_RESULT_FAILED;
// }
// } else if (((int)(cmd_mavlink.param1)) == 1) {
// /* write all parameters from RAM to EEPROM */
// int write_ret = param_save_default();
// if (write_ret == OK) {
// mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file());
// result = MAV_RESULT_ACCEPTED;
// } else {
// if (write_ret < -1) {
// mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
// } else {
// mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
// }
// result = MAV_RESULT_FAILED;
// }
// } else {
// //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
// mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request");
// result = MAV_RESULT_UNSUPPORTED;
// }
// }
// }
// /* send back command result */
// //mavlink_msg_command_ack_send(chan, cmd.command, result);
// } break;
} }
} }

23
apps/mavlink/orb_listener.c
Unescape View File

@ -114,6 +114,7 @@ static void l_manual_control_setpoint(struct listener *l);
static void l_vehicle_attitude_controls(struct listener *l); static void l_vehicle_attitude_controls(struct listener *l);
static void l_debug_key_value(struct listener *l); static void l_debug_key_value(struct listener *l);
static void l_optical_flow(struct listener *l); static void l_optical_flow(struct listener *l);
static void l_vehicle_rates_setpoint(struct listener *l);
struct listener listeners[] = { struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0}, {l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@ -136,6 +137,7 @@ struct listener listeners[] = {
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
}; };
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@ -408,6 +410,23 @@ l_attitude_setpoint(struct listener *l)
att_sp.thrust); att_sp.thrust);
} }
void
l_vehicle_rates_setpoint(struct listener *l)
{
struct vehicle_rates_setpoint_s rates_sp;
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
if (gcs_link)
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
rates_sp.timestamp/1000,
rates_sp.roll,
rates_sp.pitch,
rates_sp.yaw,
rates_sp.thrust);
}
void void
l_actuator_outputs(struct listener *l) l_actuator_outputs(struct listener *l)
{ {
@ -695,6 +714,10 @@ uorb_receive_start(void)
mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */
/* --- RATES SETPOINT VALUE --- */
mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */
/* --- ACTUATOR OUTPUTS --- */ /* --- ACTUATOR OUTPUTS --- */
mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));

2
apps/mavlink/orb_topics.h
Unescape View File

@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/optical_flow.h> #include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_controls_effective.h>
@ -79,6 +80,7 @@ struct mavlink_subscriptions {
int debug_key_value; int debug_key_value;
int input_rc_sub; int input_rc_sub;
int optical_flow; int optical_flow;
int rates_setpoint_sub;
}; };
extern struct mavlink_subscriptions mavlink_subs; extern struct mavlink_subscriptions mavlink_subs;

Write Preview
Loading…
Cancel
Save