forked from Archive/PX4-Autopilot
Added manual control abstraction layer, reworked sensors and ardrone_control apps to use it instead of direct RC channels
This commit is contained in:
parent
4eef4e1864
commit
18c6c620c0
|
@ -58,7 +58,7 @@
|
|||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/ardrone_control.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/ardrone_motors_setpoint.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
||||
|
@ -177,14 +177,14 @@ int ardrone_control_main(int argc, char *argv[])
|
|||
struct vehicle_status_s state;
|
||||
struct vehicle_attitude_s att;
|
||||
struct ardrone_control_s ar_control;
|
||||
struct rc_channels_s rc;
|
||||
struct manual_control_setpoint_s manual;
|
||||
struct sensor_combined_s raw;
|
||||
struct ardrone_motors_setpoint_s setpoint;
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
|
||||
|
||||
|
@ -194,8 +194,8 @@ int ardrone_control_main(int argc, char *argv[])
|
|||
while (1) {
|
||||
/* get a local copy of the vehicle state */
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
/* get a local copy of rc */
|
||||
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
|
||||
/* get a local copy of manual setpoint */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
|
||||
|
@ -208,7 +208,7 @@ int ardrone_control_main(int argc, char *argv[])
|
|||
position_control_thread_started = true;
|
||||
}
|
||||
|
||||
control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
|
||||
control_attitude(0, 0, 0, 0, &att, &state, ardrone_pub, &ar_control);
|
||||
|
||||
//No check for remote sticks to disarm in auto mode, land/disarm with ground station
|
||||
|
||||
|
@ -219,7 +219,8 @@ int ardrone_control_main(int argc, char *argv[])
|
|||
control_rates(&raw, &setpoint);
|
||||
|
||||
} else if (control_mode == CONTROL_MODE_ATTITUDE) {
|
||||
control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
|
||||
control_attitude(manual.roll, manual.pitch, manual.yaw,
|
||||
manual.throttle, &att, &state, ardrone_pub, &ar_control);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
|
@ -59,14 +59,19 @@ extern int gpios;
|
|||
|
||||
#define CONTROL_PID_ATTITUDE_INTERVAL 5e-3
|
||||
|
||||
void turn_xy_plane(const float_vect3 *vector, float yaw,
|
||||
float_vect3 *result);
|
||||
void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
|
||||
float_vect3 *result);
|
||||
|
||||
void turn_xy_plane(const float_vect3 *vector, float yaw,
|
||||
float_vect3 *result)
|
||||
{
|
||||
//turn clockwise
|
||||
static uint16_t counter;
|
||||
|
||||
result->x = (cos(yaw) * vector->x + sin(yaw) * vector->y);
|
||||
result->y = (-sin(yaw) * vector->x + cos(yaw) * vector->y);
|
||||
result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y);
|
||||
result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y);
|
||||
result->z = vector->z; //leave direction normal to xy-plane untouched
|
||||
|
||||
counter++;
|
||||
|
@ -84,7 +89,7 @@ void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
|
|||
// result->z = vector->z; //leave direction normal to xy-plane untouched
|
||||
}
|
||||
|
||||
void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
|
||||
void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
|
||||
{
|
||||
static int motor_skip_counter = 0;
|
||||
|
||||
|
@ -93,8 +98,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
static PID_t nick_controller;
|
||||
static PID_t roll_controller;
|
||||
|
||||
static const float min_gas = 10;
|
||||
static const float max_gas = 512;
|
||||
const float min_thrust = 0.02f; /**< 2% minimum thrust */
|
||||
const float max_thrust = 1.0f; /**< 100% max thrust */
|
||||
const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */
|
||||
|
||||
const float min_gas = min_thrust * scaling;
|
||||
const float max_gas = max_thrust * scaling;
|
||||
|
||||
static uint16_t motor_pwm[4] = {0, 0, 0, 0};
|
||||
static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
// static float remote_control_weight_z = 1;
|
||||
|
@ -160,10 +170,10 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
/* load new parameters with lower rate */
|
||||
if (motor_skip_counter % 50 == 0) {
|
||||
pid_set_parameters(&yaw_pos_controller,
|
||||
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
|
||||
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
|
||||
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
|
||||
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
|
||||
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
|
||||
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
|
||||
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
|
||||
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
|
||||
|
||||
pid_set_parameters(&yaw_speed_controller,
|
||||
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
|
||||
|
@ -189,7 +199,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
}
|
||||
|
||||
current_state = status->state_machine;
|
||||
float_vect3 attitude_setpoint_bodyframe = {}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
|
||||
float_vect3 attitude_setpoint_bodyframe = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
|
||||
|
||||
if (current_state == SYSTEM_STATE_AUTO) {
|
||||
|
||||
|
@ -200,15 +210,15 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z;
|
||||
|
||||
// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
|
||||
if (yaw_e > M_PI) {
|
||||
if (yaw_e > M_PI_F) {
|
||||
yaw_e -= 2.0f * M_PI_F;
|
||||
}
|
||||
|
||||
if (yaw_e < -M_PI) {
|
||||
if (yaw_e < -M_PI_F) {
|
||||
yaw_e += 2.0f * M_PI_F;
|
||||
}
|
||||
|
||||
attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL);
|
||||
attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0.0f, yaw_e, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL);
|
||||
|
||||
|
||||
/* limit control output */
|
||||
|
@ -234,9 +244,9 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
|
||||
|
||||
} else if (current_state == SYSTEM_STATE_MANUAL) {
|
||||
attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f;
|
||||
attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f;
|
||||
attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F;
|
||||
attitude_setpoint_bodyframe.x = -roll * M_PI_F / 8.0f;
|
||||
attitude_setpoint_bodyframe.y = -pitch * M_PI_F / 8.0f;
|
||||
attitude_setpoint_bodyframe.z = -yaw * M_PI_F;
|
||||
}
|
||||
|
||||
/* add an attitude offset which needs to be estimated somewhere */
|
||||
|
@ -245,23 +255,23 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
|
||||
/*Calculate Controllers*/
|
||||
//control Nick
|
||||
float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
|
||||
float nick_control = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
|
||||
//control Roll
|
||||
float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
|
||||
float roll_control = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
|
||||
//control Yaw Speed
|
||||
float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed!
|
||||
float yaw_rate_control = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed!
|
||||
|
||||
//compensation to keep force in z-direction
|
||||
float zcompensation;
|
||||
|
||||
if (fabs(att->roll) > 0.5f) {
|
||||
if (fabsf(att->roll) > 0.5f) {
|
||||
zcompensation = 1.13949393f;
|
||||
|
||||
} else {
|
||||
zcompensation = 1.0f / cosf(att->roll);
|
||||
}
|
||||
|
||||
if (fabs(att->pitch) > 0.5f) {
|
||||
if (fabsf(att->pitch) > 0.5f) {
|
||||
zcompensation *= 1.13949393f;
|
||||
|
||||
} else {
|
||||
|
@ -282,13 +292,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
|
||||
// FLYING MODES
|
||||
if (current_state == SYSTEM_STATE_MANUAL) {
|
||||
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;
|
||||
motor_thrust = thrust;
|
||||
|
||||
} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
|
||||
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO
|
||||
motor_thrust = thrust; //TODO
|
||||
|
||||
} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
|
||||
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO
|
||||
motor_thrust = thrust; //TODO
|
||||
|
||||
} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
|
||||
motor_thrust = 0; //immediately cut off thrust!
|
||||
|
@ -301,39 +311,37 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
|
||||
// Convertion to motor-step units
|
||||
motor_thrust *= zcompensation;
|
||||
/* scale up from 0..1 to 10..512) */
|
||||
motor_thrust *= ((float)max_gas - min_gas);
|
||||
|
||||
//limit control output
|
||||
//yawspeed
|
||||
if (yaw > pid_yawspeed_lim) {
|
||||
yaw = pid_yawspeed_lim;
|
||||
if (yaw_rate_control > pid_yawspeed_lim) {
|
||||
yaw_rate_control = pid_yawspeed_lim;
|
||||
yaw_speed_controller.saturated = 1;
|
||||
}
|
||||
|
||||
if (yaw < -pid_yawspeed_lim) {
|
||||
yaw = -pid_yawspeed_lim;
|
||||
if (yaw_rate_control < -pid_yawspeed_lim) {
|
||||
yaw_rate_control = -pid_yawspeed_lim;
|
||||
yaw_speed_controller.saturated = 1;
|
||||
}
|
||||
|
||||
if (nick > pid_att_lim) {
|
||||
nick = pid_att_lim;
|
||||
if (nick_control > pid_att_lim) {
|
||||
nick_control = pid_att_lim;
|
||||
nick_controller.saturated = 1;
|
||||
}
|
||||
|
||||
if (nick < -pid_att_lim) {
|
||||
nick = -pid_att_lim;
|
||||
if (nick_control < -pid_att_lim) {
|
||||
nick_control = -pid_att_lim;
|
||||
nick_controller.saturated = 1;
|
||||
}
|
||||
|
||||
|
||||
if (roll > pid_att_lim) {
|
||||
roll = pid_att_lim;
|
||||
if (roll_control > pid_att_lim) {
|
||||
roll_control = pid_att_lim;
|
||||
roll_controller.saturated = 1;
|
||||
}
|
||||
|
||||
if (roll < -pid_att_lim) {
|
||||
roll = -pid_att_lim;
|
||||
if (roll_control < -pid_att_lim) {
|
||||
roll_control = -pid_att_lim;
|
||||
roll_controller.saturated = 1;
|
||||
}
|
||||
|
||||
|
@ -342,44 +350,44 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x;
|
||||
ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y;
|
||||
ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z;
|
||||
ar_control->attitude_control_output[0] = roll;
|
||||
ar_control->attitude_control_output[1] = nick;
|
||||
ar_control->attitude_control_output[2] = yaw;
|
||||
ar_control->attitude_control_output[0] = roll_control;
|
||||
ar_control->attitude_control_output[1] = nick_control;
|
||||
ar_control->attitude_control_output[2] = yaw_rate_control;
|
||||
ar_control->zcompensation = zcompensation;
|
||||
orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control);
|
||||
|
||||
static float output_band = 0.f;
|
||||
static float band_factor = 0.75f;
|
||||
static float startpoint_full_control = 150.0f; //TODO
|
||||
static float yaw_factor = 1.0f;
|
||||
float output_band = 0.f;
|
||||
float band_factor = 0.75f;
|
||||
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
|
||||
float yaw_factor = 1.0f;
|
||||
|
||||
if (motor_thrust <= min_gas) {
|
||||
motor_thrust = min_gas;
|
||||
if (motor_thrust <= min_thrust) {
|
||||
motor_thrust = min_thrust;
|
||||
output_band = 0.f;
|
||||
|
||||
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) {
|
||||
output_band = band_factor * (motor_thrust - min_gas);
|
||||
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
|
||||
output_band = band_factor * (motor_thrust - min_thrust);
|
||||
|
||||
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) {
|
||||
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
|
||||
output_band = band_factor * startpoint_full_control;
|
||||
|
||||
} else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) {
|
||||
output_band = band_factor * (max_gas - motor_thrust);
|
||||
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
|
||||
output_band = band_factor * (max_thrust - motor_thrust);
|
||||
}
|
||||
|
||||
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
|
||||
|
||||
// FRONT (MOTOR 1)
|
||||
motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw);
|
||||
motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control);
|
||||
|
||||
// RIGHT (MOTOR 2)
|
||||
motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw);
|
||||
motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control);
|
||||
|
||||
// BACK (MOTOR 3)
|
||||
motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw);
|
||||
motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control);
|
||||
|
||||
// LEFT (MOTOR 4)
|
||||
motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw);
|
||||
motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control);
|
||||
|
||||
// if we are not in the output band
|
||||
if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
|
||||
|
@ -389,16 +397,16 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
|
||||
yaw_factor = 0.5f;
|
||||
// FRONT (MOTOR 1)
|
||||
motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor);
|
||||
motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor);
|
||||
|
||||
// RIGHT (MOTOR 2)
|
||||
motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor);
|
||||
motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control * yaw_factor);
|
||||
|
||||
// BACK (MOTOR 3)
|
||||
motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor);
|
||||
motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control * yaw_factor);
|
||||
|
||||
// LEFT (MOTOR 4)
|
||||
motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor);
|
||||
motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor);
|
||||
}
|
||||
|
||||
uint8_t i;
|
||||
|
@ -414,14 +422,17 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
|
|||
}
|
||||
}
|
||||
|
||||
// Write out actual thrust
|
||||
motor_pwm[0] = (uint16_t) motor_calc[0];
|
||||
motor_pwm[1] = (uint16_t) motor_calc[1];
|
||||
motor_pwm[2] = (uint16_t) motor_calc[2];
|
||||
motor_pwm[3] = (uint16_t) motor_calc[3];
|
||||
/* set the motor values */
|
||||
|
||||
// Keep motors spinning while armed
|
||||
/* scale up from 0..1 to 10..512) */
|
||||
motor_pwm[0] = (uint16_t) motor_calc[0] * ((float)max_gas - min_gas) + min_gas;
|
||||
motor_pwm[1] = (uint16_t) motor_calc[1] * ((float)max_gas - min_gas) + min_gas;
|
||||
motor_pwm[2] = (uint16_t) motor_calc[2] * ((float)max_gas - min_gas) + min_gas;
|
||||
motor_pwm[3] = (uint16_t) motor_calc[3] * ((float)max_gas - min_gas) + min_gas;
|
||||
|
||||
/* Keep motors spinning while armed and prevent overflows */
|
||||
|
||||
/* Failsafe logic - should never be necessary */
|
||||
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
|
||||
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
|
||||
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
|
||||
|
|
|
@ -45,7 +45,7 @@
|
|||
#include <uORB/topics/ardrone_control.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att,
|
||||
void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att,
|
||||
const struct vehicle_status_s *status, int ardrone_pub,
|
||||
struct ardrone_control_s *ar_control);
|
||||
|
||||
|
|
|
@ -631,8 +631,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
|||
|
||||
}
|
||||
|
||||
|
||||
static void *command_handling_loop(void *arg) //handles commands which come from the mavlink app
|
||||
/**
|
||||
* Handle commands sent by the ground control station via MAVLink.
|
||||
*/
|
||||
static void *command_handling_loop(void *arg)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "commander cmd handler", getpid());
|
||||
|
@ -645,7 +647,7 @@ static void *command_handling_loop(void *arg) //handles commands which come fro
|
|||
struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } };
|
||||
|
||||
if (poll(fds, 1, 5000) == 0) {
|
||||
/* timeout, but this is no problem */
|
||||
/* timeout, but this is no problem, silently ignore */
|
||||
} else {
|
||||
/* got command */
|
||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||
|
@ -785,7 +787,8 @@ int commander_main(int argc, char *argv[])
|
|||
stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status);
|
||||
|
||||
if (stat_pub < 0) {
|
||||
printf("[commander] ERROR: orb_advertise failed.\n");
|
||||
printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
/* make sure we are in preflight state */
|
||||
|
@ -1035,6 +1038,8 @@ int commander_main(int argc, char *argv[])
|
|||
int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale;
|
||||
int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale;
|
||||
int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
|
||||
/* Check the value of the rc channel of the mode switch */
|
||||
mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
|
||||
|
||||
/* check if left stick is in lower left position --> switch to standby state */
|
||||
if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
|
||||
|
@ -1061,9 +1066,6 @@ int commander_main(int argc, char *argv[])
|
|||
}
|
||||
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
|
||||
|
||||
/* Check the value of the rc channel of the mode switch */
|
||||
mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
|
||||
|
||||
if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
|
||||
update_state_machine_mode_manual(stat_pub, ¤t_status);
|
||||
|
||||
|
@ -1074,6 +1076,9 @@ int commander_main(int argc, char *argv[])
|
|||
update_state_machine_mode_stabilized(stat_pub, ¤t_status);
|
||||
}
|
||||
|
||||
/* Publish RC signal */
|
||||
|
||||
|
||||
/* handle the case where RC signal was regained */
|
||||
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
|
||||
current_status.rc_signal_lost = false;
|
||||
|
|
|
@ -63,6 +63,7 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "sensors.h"
|
||||
|
@ -383,10 +384,23 @@ int sensors_main(int argc, char *argv[])
|
|||
pthread_mutex_init(&sensors_read_ready_mutex, NULL);
|
||||
pthread_cond_init(&sensors_read_ready, NULL);
|
||||
|
||||
/* advertise the topic and make the initial publication */
|
||||
/* advertise the sensor_combined topic and make the initial publication */
|
||||
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
|
||||
publishing = true;
|
||||
|
||||
/* advertise the manual_control topic */
|
||||
struct manual_control_setpoint_s manual_control = { .mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE,
|
||||
.roll = 0.0f,
|
||||
.pitch = 0.0f,
|
||||
.yaw = 0.0f,
|
||||
.throttle = 0.0f };
|
||||
|
||||
int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
|
||||
|
||||
if (manual_control_pub < 0) {
|
||||
printf("[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
|
||||
}
|
||||
|
||||
/* advertise the rc topic */
|
||||
struct rc_channels_s rc;
|
||||
memset(&rc, 0, sizeof(rc));
|
||||
|
@ -451,23 +465,23 @@ int sensors_main(int argc, char *argv[])
|
|||
|
||||
|
||||
/* Update RC scalings and function mappings */
|
||||
rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
|
||||
rc.chan[0].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
|
||||
* global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]);
|
||||
rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM];
|
||||
|
||||
rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
|
||||
rc.chan[1].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
|
||||
* global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]);
|
||||
rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM];
|
||||
|
||||
rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
|
||||
rc.chan[2].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
|
||||
* global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]);
|
||||
rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM];
|
||||
|
||||
rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
|
||||
rc.chan[3].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
|
||||
* global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]);
|
||||
rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM];
|
||||
|
||||
rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
|
||||
rc.chan[4].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
|
||||
* global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]);
|
||||
rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM];
|
||||
|
||||
|
@ -708,20 +722,48 @@ int sensors_main(int argc, char *argv[])
|
|||
|
||||
/* require at least two channels
|
||||
* to consider the signal valid
|
||||
* check that decoded measurement is up to date
|
||||
*/
|
||||
if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
|
||||
/* Read out values from HRT */
|
||||
for (int i = 0; i < ppm_decoded_channels; i++) {
|
||||
rc.chan[i].raw = ppm_buffer[i];
|
||||
/* Set the range to +-, then scale up */
|
||||
rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
|
||||
rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000;
|
||||
rc.chan[i].scaled = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
|
||||
}
|
||||
|
||||
rc.chan_count = ppm_decoded_channels;
|
||||
|
||||
rc.timestamp = ppm_last_valid_decode;
|
||||
|
||||
/* publish a few lines of code later if set to true */
|
||||
ppm_updated = true;
|
||||
|
||||
/* roll input */
|
||||
manual_control.roll = rc.chan[rc.function[ROLL]].scaled;
|
||||
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
|
||||
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
|
||||
|
||||
/* pitch input */
|
||||
manual_control.pitch = rc.chan[rc.function[PITCH]].scaled;
|
||||
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
|
||||
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
|
||||
|
||||
/* yaw input */
|
||||
manual_control.yaw = rc.chan[rc.function[YAW]].scaled;
|
||||
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
|
||||
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
|
||||
|
||||
/* throttle input */
|
||||
manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled;
|
||||
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
|
||||
if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f;
|
||||
|
||||
/* mode switch input */
|
||||
manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled;
|
||||
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
|
||||
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
|
||||
|
||||
}
|
||||
ppmcounter = 0;
|
||||
}
|
||||
|
@ -832,6 +874,7 @@ int sensors_main(int argc, char *argv[])
|
|||
|
||||
if (ppm_updated) {
|
||||
orb_publish(ORB_ID(rc_channels), rc_pub, &rc);
|
||||
orb_publish(ORB_ID(manual_control_setpoint), manual_control_pub, &manual_control);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -100,7 +100,10 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
|
|||
ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
|
||||
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
|
||||
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
|
||||
|
||||
#include "topics/manual_control_setpoint.h"
|
||||
ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
|
||||
|
||||
#include "topics/actuator_controls.h"
|
||||
ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
|
||||
|
|
|
@ -0,0 +1,95 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file manual_control_setpoint.h
|
||||
* Definition of the manual_control_setpoint uORB topic.
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
|
||||
#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Defines how RC channels map to control inputs.
|
||||
*
|
||||
* The default mode on quadrotors and fixed wing is
|
||||
* roll and pitch position of the right stick and
|
||||
* throttle and yaw rate on the left stick
|
||||
*/
|
||||
enum MANUAL_CONTROL_MODE
|
||||
{
|
||||
DIRECT = 0,
|
||||
ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1,
|
||||
ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2,
|
||||
ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3
|
||||
};
|
||||
|
||||
struct manual_control_setpoint_s {
|
||||
|
||||
enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */
|
||||
float roll; /**< roll / roll rate input */
|
||||
float pitch;
|
||||
float yaw;
|
||||
float throttle;
|
||||
|
||||
float override_mode_switch;
|
||||
|
||||
float ailerons;
|
||||
float elevator;
|
||||
float rudder;
|
||||
float flaps;
|
||||
|
||||
float aux1_cam_pan;
|
||||
float aux2_cam_tilt;
|
||||
float aux3_cam_zoom;
|
||||
float aux4_cam_roll;
|
||||
|
||||
}; /**< manual control inputs */
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(manual_control_setpoint);
|
||||
|
||||
#endif
|
|
@ -61,8 +61,14 @@ struct vehicle_attitude_setpoint_s
|
|||
float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */
|
||||
float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */
|
||||
|
||||
float roll_body; /**< body angle in NED frame */
|
||||
float pitch_body; /**< body angle in NED frame */
|
||||
float yaw_body; /**< body angle in NED frame */
|
||||
float body_valid; /**< Set to true if Tait-Bryan angles are valid */
|
||||
|
||||
float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
|
||||
bool R_valid; /**< Set to true if rotation matrix is valid */
|
||||
|
||||
float thrust; /**< Thrust in Newton the power system should generate */
|
||||
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue