diff options
Diffstat (limited to 'apps/mavlink')
-rw-r--r-- | apps/mavlink/mavlink.c | 1 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.c | 78 | ||||
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 21 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 45 | ||||
-rw-r--r-- | apps/mavlink/orb_topics.h | 2 |
5 files changed, 58 insertions, 89 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 81b907bcd..77034914f 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -745,6 +745,7 @@ int mavlink_main(int argc, char *argv[]) thread_should_exit = true; while (thread_running) { usleep(200000); + printf("."); } warnx("terminated."); exit(0); diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 7cb1582b4..9d9b9914a 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -72,20 +72,6 @@ static unsigned int mavlink_param_queue_index = 0; */ 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) { mavlink_pm_send_param(param); @@ -232,69 +218,5 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } 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; } } diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index dd011aeed..fb59d07a1 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -298,6 +298,26 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); + /* Calculate Rotation Matrix */ + //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode + + if(mavlink_system.type == MAV_TYPE_FIXED_WING) { + //TODO: assuming low pitch and roll values for now + hil_attitude.R[0][0] = cosf(hil_state.yaw); + hil_attitude.R[0][1] = sinf(hil_state.yaw); + hil_attitude.R[0][2] = 0.0f; + + hil_attitude.R[1][0] = -sinf(hil_state.yaw); + hil_attitude.R[1][1] = cosf(hil_state.yaw); + hil_attitude.R[1][2] = 0.0f; + + hil_attitude.R[2][0] = 0.0f; + hil_attitude.R[2][1] = 0.0f; + hil_attitude.R[2][2] = 1.0f; + + hil_attitude.R_valid = true; + } + hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -305,6 +325,7 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; + /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index a067460d8..b0aa401fc 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -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_debug_key_value(struct listener *l); static void l_optical_flow(struct listener *l); +static void l_vehicle_rates_setpoint(struct listener *l); struct listener listeners[] = { {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_debug_key_value, &mavlink_subs.debug_key_value, 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]); @@ -409,6 +411,23 @@ l_attitude_setpoint(struct listener *l) } 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 l_actuator_outputs(struct listener *l) { struct actuator_outputs_s act_outputs; @@ -547,28 +566,28 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_controls_s actuators; + struct actuator_controls_effective_s actuators; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); + "eff ctrl0 ", + actuators.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); + "eff ctrl1 ", + actuators.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); + "eff ctrl2 ", + actuators.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + "eff ctrl3 ", + actuators.control_effective[3]); } } @@ -695,6 +714,10 @@ uorb_receive_start(void) mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); 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 --- */ mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); @@ -716,7 +739,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index 61e664401..4650c4991 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_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/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -79,6 +80,7 @@ struct mavlink_subscriptions { int debug_key_value; int input_rc_sub; int optical_flow; + int rates_setpoint_sub; }; extern struct mavlink_subscriptions mavlink_subs; |