diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c8564792f8..f5e143066a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -742,6 +742,31 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; + case PX4_CMD_CONTROLLER_SELECTION: { + bool changed = false; + if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) { + current_vehicle_status->flag_control_rates_enabled = cmd->param1; + changed = true; + } + if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) { + current_vehicle_status->flag_control_attitude_enabled = cmd->param2; + changed = true; + } + if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) { + current_vehicle_status->flag_control_velocity_enabled = cmd->param3; + changed = true; + } + if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) { + current_vehicle_status->flag_control_position_enabled = cmd->param4; + changed = true; + } + + if (changed) { + /* publish current state machine */ + state_machine_publish(status_pub, current_vehicle_status, mavlink_fd); + } + } + // /* request to land */ // case MAV_CMD_NAV_LAND: // { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index fb96866b0d..6470e65c89 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,14 +126,13 @@ static struct vehicle_attitude_s hil_attitude; static struct vehicle_global_position_s hil_global_pos; -static struct ardrone_motors_setpoint_s ardrone_motors; +static struct vehicle_rates_setpoint_s vehicle_rates_sp; static struct vehicle_command_s vcmd; static struct actuator_armed_s armed; static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t ardrone_motors_pub = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t global_position_setpoint_pub = -1; @@ -188,6 +187,12 @@ static struct mavlink_subscriptions { .initialized = false }; +static struct mavlink_publications { + orb_advert_t vehicle_rates_sp_pub; +} mavlink_pubs = { + .vehicle_rates_sp_pub = -1 +}; + /* 3: Define waypoint helper functions */ void mavlink_wpm_send_message(mavlink_message_t *msg); @@ -1133,6 +1138,13 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } +#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0 +#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1 +#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2 +#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3 +#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4 +#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -1242,29 +1254,80 @@ void handleMessage(mavlink_message_t *msg) // printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid); if (mavlink_system.sysid < 4) { - ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid]; - ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid]; - ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid]; - ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid]; - - ardrone_motors.timestamp = hrt_absolute_time(); - /* only send if RC is off */ if (v_status.rc_signal_lost) { - /* check if input has to be enabled */ - if (!v_status.flag_control_offboard_enabled) { - /* XXX Enable offboard control */ - } - /* XXX decode mode and set flags */ - // if (mode == abc) xxx flag_control_rates_enabled; + /* rate control mode */ + if (quad_motors_setpoint.mode == 1) { + vehicle_rates_sp.roll = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + vehicle_rates_sp.pitch = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + vehicle_rates_sp.yaw = quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + vehicle_rates_sp.thrust = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX; + + vehicle_rates_sp.timestamp = hrt_absolute_time(); + } /* check if topic has to be advertised */ - if (ardrone_motors_pub <= 0) { - ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors); + if (mavlink_pubs.vehicle_rates_sp_pub <= 0) { + mavlink_pubs.vehicle_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &vehicle_rates_sp); } /* Publish */ - orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors); + orb_publish(ORB_ID(vehicle_rates_setpoint), mavlink_pubs.vehicle_rates_sp_pub, &vehicle_rates_sp); + + /* change armed status if required */ + bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); + + bool cmd_generated = false; + + if (v_status.flag_control_offboard_enabled != cmd_armed) { + vcmd.param1 = cmd_armed; + vcmd.param2 = 0; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; + vcmd.target_system = mavlink_system.sysid; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + cmd_generated = true; + } + + /* check if input has to be enabled */ + if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || + (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || + (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || + (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { + vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); + vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); + vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); + vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = PX4_CMD_CONTROLLER_SELECTION; + vcmd.target_system = mavlink_system.sysid; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + cmd_generated = true; + } + + if (cmd_generated) { + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } } } } @@ -1479,7 +1542,7 @@ int mavlink_thread_main(int argc, char *argv[]) memset(&rc, 0, sizeof(rc)); memset(&hil_attitude, 0, sizeof(hil_attitude)); memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&ardrone_motors, 0, sizeof(ardrone_motors)); + memset(&vehicle_rates_sp, 0, sizeof(vehicle_rates_sp)); memset(&vcmd, 0, sizeof(vcmd)); /* print welcome text */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5300337015..3f0d562459 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -58,13 +58,14 @@ #include #include #include -#include +#include #include #include #include #include "multirotor_attitude_control.h" +#include "multirotor_rate_control.h" __EXPORT int multirotor_att_control_main(int argc, char *argv[]); @@ -93,18 +94,18 @@ mc_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - struct ardrone_motors_setpoint_s setpoint; - memset(&setpoint, 0, sizeof(setpoint)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); struct actuator_controls_s actuators; /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); 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)); /* * Do not rate-limit the loop to prevent aliasing @@ -145,6 +146,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of rates setpoint */ + bool updated; + orb_check(setpoint_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp); + } /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -155,16 +162,15 @@ mc_thread_main(int argc, char *argv[]) /* manual inputs, from RC control or joystick */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_rate_body = manual.yaw; + att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; att_sp.timestamp = hrt_absolute_time(); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.roll_rate_body = 0.0f; - att_sp.pitch_rate_body = 0.0f; - att_sp.yaw_rate_body = 0.0f; att_sp.thrust = 0.1f; } else { att_sp.thrust = manual.throttle; @@ -184,7 +190,14 @@ mc_thread_main(int argc, char *argv[]) /* XXX could be also run in an independent loop */ if (state.flag_control_rates_enabled) { - multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators); + /* lowpass gyros */ + // XXX + static float gyro_lp[3] = {0, 0, 0}; + gyro_lp[0] = raw.gyro_rad_s[0]; + gyro_lp[1] = raw.gyro_rad_s[1]; + gyro_lp[2] = raw.gyro_rad_s[2]; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); } /* STEP 3: publish the result to the vehicle actuators */ diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 997aa0c108..d7413913bd 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, att->roll, att->rollspeed, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 8a3ac78cdc..fefd1f7679 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru return OK; } -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { static uint64_t last_run = 0; @@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body, + float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body, + float roll_control = pid_calculate(&roll_controller, rate_sp->roll, rates[0], 0.0f, deltaT); /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT); + float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); /* * compensate the vertical loss of thrust diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h index 6f34d99e7a..3c3c508014 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.h +++ b/apps/multirotor_att_control/multirotor_rate_control.h @@ -47,10 +47,10 @@ #include #include -#include +#include #include -void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp, +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 46f86a348d..07426ec2bd 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/apps/px4/ground_estimator/ground_estimator.c @@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) { /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* advertise attitude */ + /* advertise debug value */ struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 1cad57a436..be85a345ae 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -78,8 +78,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); #include "topics/vehicle_local_position.h" ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); -#include "topics/ardrone_motors_setpoint.h" -ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s); +#include "topics/vehicle_rates_setpoint.h" +ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 0c33ebff8c..62367cf77a 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -56,21 +56,18 @@ struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ - 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_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ + //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 roll_rate_body; /**< body angle in NED frame */ - float pitch_rate_body; /**< body angle in NED frame */ - float yaw_rate_body; /**< body angle in NED frame */ - float body_valid; /**< Set to true if Tait-Bryan angles are valid */ + //float body_valid; /**< Set to true if body 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 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 */ diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h index dca928efd2..3e220965d9 100644 --- a/apps/uORB/topics/vehicle_command.h +++ b/apps/uORB/topics/vehicle_command.h @@ -50,6 +50,10 @@ * @{ */ +enum PX4_CMD { + PX4_CMD_CONTROLLER_SELECTION = 1000, +}; + struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ diff --git a/apps/uORB/topics/ardrone_motors_setpoint.h b/apps/uORB/topics/vehicle_rates_setpoint.h similarity index 70% rename from apps/uORB/topics/ardrone_motors_setpoint.h rename to apps/uORB/topics/vehicle_rates_setpoint.h index d0cb0ad733..3400e53999 100644 --- a/apps/uORB/topics/ardrone_motors_setpoint.h +++ b/apps/uORB/topics/vehicle_rates_setpoint.h @@ -33,12 +33,12 @@ ****************************************************************************/ /** - * @file ardrone_motors_setpoint.h - * Definition of the ardrone_motors_setpoint uORB topic. + * @file vehicle_rates_setpoint.h + * Definition of the vehicle rates setpoint topic */ -#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_ -#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_ +#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ +#define TOPIC_VEHICLE_RATES_SETPOINT_H_ #include #include "../uORB.h" @@ -48,23 +48,22 @@ * @{ */ -struct ardrone_motors_setpoint_s +struct vehicle_rates_setpoint_s { - uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + uint64_t timestamp; /**< in microseconds since system start */ - uint8_t group; /**< quadrotor group */ - uint8_t mode; /**< requested flight mode XXX define */ - float p1; /**< parameter 1: rate control: roll rate, att control: roll angle (in radians, NED) */ - float p2; /**< parameter 2: rate control: pitch rate, att control: pitch angle (in radians, NED) */ - float p3; /**< parameter 3: rate control: yaw rate, att control: yaw angle (in radians, NED) */ - float p4; /**< parameter 4: thrust, [0..1] */ -}; /**< AR.Drone low level motors */ + float roll; /**< body angular rates in NED frame */ + float pitch; /**< body angular rates in NED frame */ + float yaw; /**< body angular rates in NED frame */ + float thrust; /**< thrust normalized to 0..1 */ + +}; /**< vehicle_rates_setpoint */ /** * @} */ /* register this as object request broker structure */ -ORB_DECLARE(ardrone_motors_setpoint); +ORB_DECLARE(vehicle_rates_setpoint); #endif diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index cfde2ab53c..850029f015 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -127,7 +127,7 @@ struct vehicle_status_s bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_speed_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */