Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Feb 5, 2017
1 parent 05d6672 commit 404fb95
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 5 deletions.
2 changes: 1 addition & 1 deletion src/modules/commander/commander.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -524,7 +524,7 @@ int commander_main(int argc, char *argv[])
cmd.target_system = status.system_id;
cmd.target_component = status.component_id;
cmd.command = vehicle_command_s::VEHICLE_CMD_POSCTRL_MODE_CIRCLE;
cmd.param1 = 50.0f;
cmd.param1 = 20.0f;
cmd.param2 = NAN;
cmd.param3 = NAN;
cmd.param4 = NAN;
Expand Down
19 changes: 15 additions & 4 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -846,6 +846,16 @@ MulticopterPositionControl::poll_subscriptions()

} else if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_POSCTRL_MODE_CIRCLE) {


PX4_WARN("param1 (radius) %.3f", (double)_vehicle_command.param1);
PX4_WARN("param2 (velocity) %.3f", (double)_vehicle_command.param2);
PX4_WARN("param3 (altitude) %.3f", (double)_vehicle_command.param3);
PX4_WARN("param4 %.3f", (double)_vehicle_command.param4);
PX4_WARN("param5 (lat) %.3f", (double)_vehicle_command.param5);
PX4_WARN("param6 (lon) %.3f", (double)_vehicle_command.param6);
PX4_WARN("param7 %.3f", (double)_vehicle_command.param7);


_posctrl_sub_mode = MODE_CIRCLE;

// circle radius
Expand All @@ -859,7 +869,7 @@ MulticopterPositionControl::poll_subscriptions()
_R_circle = math::constrain(_R_circle, CIRCLE_RADIUS_DEFAULT, CIRCLE_RADIUS_MAX);


// tanential velocity
// tangential velocity
// if tangential velocity is non-finite then manual control inputs from RC will be used to generate tangential velocity
if (PX4_ISFINITE(_vehicle_command.param2)) {
_v_tang_circle_sp = math::constrain(_vehicle_command.param2, -_params.vel_cruise(0), _params.vel_cruise(0));
Expand All @@ -868,14 +878,15 @@ MulticopterPositionControl::poll_subscriptions()
_v_tang_circle_sp = _params.vel_cruise(0);
}

// altitude
// param3 optional altitude
if (PX4_ISFINITE(_vehicle_command.param3)) {
_pos_sp(2) = -_vehicle_command.param3;
}

// param5 lat
// param6 lon
if (PX4_ISFINITE(_vehicle_command.param5) && PX4_ISFINITE(_vehicle_command.param6)) {
map_projection_project(&_ref_pos,
_vehicle_command.param5, _vehicle_command.param6,
map_projection_project(&_ref_pos, _vehicle_command.param5, _vehicle_command.param6,
&_circle_orig.data[0], &_circle_orig.data[1]);

} else {
Expand Down

3 comments on commit 404fb95

@aysh-shariff
Copy link

@aysh-shariff aysh-shariff commented on 404fb95 Mar 7, 2017 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dagar Is it possible to get the copter to circle the target in follow mode depending on the velocity of the target? Say, the copter circles the target only when the target has stopped moving.

@mhkabir
Copy link
Member

@mhkabir mhkabir commented on 404fb95 Mar 7, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You would need to add support yourself.

@aysh-shariff
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@mhkabir Yes ofcourse. But I could do with some help to get me started.

Please sign in to comment.