fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 15:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'actuator_eff_fix' into mavlink_fixes

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-16 23:39:56 +04:00
parent 6ed268aa28 63d81ba415
commit 6cf09183d6
9 changed files with 42 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/orb_listener.c
+1 -33
View File
@@ -73,7 +73,6 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct airspeed_s airspeed;
@@ -120,7 +119,6 @@ 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_effective(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);
@@ -148,7 +146,6 @@ static const struct listener listeners[] = {
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_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},
@@ -250,7 +247,7 @@ l_vehicle_attitude(const struct listener *l)
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = actuators_effective_0.control_effective[3] * 100.0f;
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
}
}
@@ -586,32 +583,6 @@ l_manual_control_setpoint(const struct listener *l)
0);
}
void
l_vehicle_attitude_controls_effective(const struct listener *l)
{
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
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,
"eff ctrl0 ",
actuators_effective_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
actuators_effective_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
actuators_effective_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
actuators_effective_0.control_effective[3]);
}
}
void
l_vehicle_attitude_controls(const struct listener *l)
{
@@ -822,9 +793,6 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
src/modules/px4iofirmware/mixer.cpp
+5 -1
View File
@@ -185,7 +185,7 @@ mixer_tick(void)
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
@@ -201,6 +201,10 @@ mixer_tick(void)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
src/modules/sdlog2/sdlog2.c
+3 -18
View File
@@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
@@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[])
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_effective_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
@@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* Sanity check variable and index */
/* number of subscriptions */
const ssize_t fdsc = 19;
/* sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE --- */
subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.act_controls_effective_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
@@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- ACTUATOR CONTROL EFFECTIVE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
// TODO not implemented yet
}
/* --- LOCAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
src/modules/uORB/objects_common.cpp
-7
View File
@@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
/* actuator controls, as set by actuators / mixers after limiting */
#include "topics/actuator_controls_effective.h"
ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
#include "topics/actuator_outputs.h"
ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
src/modules/uORB/topics/actuator_controls_effective.h
+30 -30
View File
@@ -46,34 +46,34 @@
#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
#include <stdint.h>
#include "../uORB.h"
#include "actuator_controls.h"
//#include <stdint.h>
//#include "../uORB.h"
//#include "actuator_controls.h"
//
//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
//
///**
// * @addtogroup topics
// * @{
// */
//
//struct actuator_controls_effective_s {
// uint64_t timestamp;
// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
//};
//
///**
// * @}
// */
//
///* actuator control sets; this list can be expanded as more controllers emerge */
//ORB_DECLARE(actuator_controls_effective_0);
//ORB_DECLARE(actuator_controls_effective_1);
//ORB_DECLARE(actuator_controls_effective_2);
//ORB_DECLARE(actuator_controls_effective_3);
//
///* control sets with pre-defined applications */
//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0)
#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
/**
* @addtogroup topics
* @{
*/
struct actuator_controls_effective_s {
uint64_t timestamp;
float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
};
/**
* @}
*/
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_effective_0);
ORB_DECLARE(actuator_controls_effective_1);
ORB_DECLARE(actuator_controls_effective_2);
ORB_DECLARE(actuator_controls_effective_3);
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0)
#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */