CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Set correct order for pitch and roll everywhere to match XYZ convention

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-15 19:35:23 +03:00
parent c004165eb0
commit 323c1da325
14 changed files with 44 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
clover/src/autotest/autotest_flight.py
View File

@@ -74,12 +74,12 @@ navigate(yaw=-math.pi / 2, frame_id='navigate_target')
rospy.sleep(3)
input('Use set_attitude to fly backwards [enter]')
set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body')
rospy.sleep(0.3)
set_position(frame_id='body')
input('Use set_attitude to fly right [enter]')
set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body')
rospy.sleep(0.5)
set_position(frame_id='body')

16
clover/src/simple_offboard.cpp
View File

@@ -101,7 +101,7 @@ PoseStamped nav_start;
PointStamped setpoint_position;
PointStamped setpoint_altitude;
Vector3Stamped setpoint_velocity;
float setpoint_yaw, setpoint_pitch, setpoint_roll;
float setpoint_yaw, setpoint_roll, setpoint_pitch;
Vector3 setpoint_rates;
string yaw_frame_id;
float setpoint_thrust;
@@ -216,11 +216,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
res.vx = NAN;
res.vy = NAN;
res.vz = NAN;
res.pitch = NAN;
res.roll = NAN;
res.pitch = NAN;
res.yaw = NAN;
res.pitch_rate = NAN;
res.roll_rate = NAN;
res.pitch_rate = NAN;
res.yaw_rate = NAN;
res.voltage = NAN;
res.cell_voltage = NAN;
@@ -659,7 +659,7 @@ inline float safe(float value) {
#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); }
bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line
float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
uint8_t& success, string& message) // editorconfig-checker-disable-line
{
@@ -699,12 +699,12 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
ENSURE_NON_INF(vy);
ENSURE_NON_INF(vz);
} else if (sp_type == ATTITUDE) {
ENSURE_NON_INF(pitch);
ENSURE_NON_INF(roll);
ENSURE_NON_INF(pitch);
ENSURE_NON_INF(thrust);
} else if (sp_type == RATES) {
ENSURE_NON_INF(pitch_rate);
ENSURE_NON_INF(roll_rate);
ENSURE_NON_INF(pitch_rate);
ENSURE_NON_INF(yaw_rate);
ENSURE_NON_INF(thrust);
}
@@ -968,11 +968,11 @@ bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
}
bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
}
bool setRates(SetRates::Request& req, SetRates::Response& res) {
return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
}
bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)

4
clover/srv/GetTelemetry.srv
View File

@@ -13,11 +13,11 @@ float32 alt
float32 vx
float32 vy
float32 vz
float32 pitch
float32 roll
float32 pitch
float32 yaw
float32 pitch_rate
float32 roll_rate
float32 pitch_rate
float32 yaw_rate
float32 voltage
float32 cell_voltage

2
clover/srv/SetAttitude.srv
View File

@@ -1,5 +1,5 @@
float32 pitch
float32 roll
float32 pitch
float32 yaw
float32 thrust
string frame_id

2
clover/srv/SetRates.srv
View File

@@ -1,5 +1,5 @@
float32 pitch_rate
float32 roll_rate
float32 pitch_rate
float32 yaw_rate
float32 thrust
bool auto_arm

12
clover_blocks/www/blocks.js
View File

@@ -59,8 +59,8 @@ function updateSetpointBlock(e) {
this.getInput('VY').setVisible(velocity);
this.getInput('VZ').setVisible(velocity);
this.getInput('YAW').setVisible(attitude);
this.getInput('PITCH').setVisible(attitude);
this.getInput('ROLL').setVisible(attitude);
this.getInput('PITCH').setVisible(attitude);
this.getInput('THRUST').setVisible(attitude);
this.getInput('RELATIVE_TO').setVisible(type != 'RATES');
@@ -163,14 +163,14 @@ Blockly.Blocks['setpoint'] = {
this.appendValueInput("VZ")
.setCheck("Number")
.appendField("vz");
this.appendValueInput("PITCH")
.setCheck("Number")
.appendField("pitch")
.setVisible(false);
this.appendValueInput("ROLL")
.setCheck("Number")
.appendField("roll")
.setVisible(false);
this.appendValueInput("PITCH")
.setCheck("Number")
.appendField("pitch")
.setVisible(false);
this.appendValueInput("YAW")
.setCheck("Number")
.appendField("yaw")
@@ -247,7 +247,7 @@ Blockly.Blocks['get_attitude'] = {
init: function () {
this.appendDummyInput()
.appendField("current")
.appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
.appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
this.setOutput(true, "Number");
this.setColour(COLOR_STATE);
this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian).");

2
clover_blocks/www/index.html
View File

@@ -69,8 +69,8 @@
<value name="VX"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="VY"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="VZ"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="YAW"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="THRUST"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>

4
clover_blocks/www/python.js
View File

@@ -328,11 +328,11 @@ Blockly.Python.setpoint = function(block) {
} else if (type == 'ATTITUDE') {
rosDefinitions.setAttitude = true;
simpleOffboard();
return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
} else if (type == 'RATES') {
rosDefinitions.setRates = true;
simpleOffboard();
return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`;
return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`;
}
}

4
docs/en/parameters.md
View File

@@ -60,8 +60,8 @@ The `SYS_MC_EST_GROUP` parameter defines the estimator subsystem to use.
Estimator subsystem is a group of modules that calculates the current state of the copter using readings from the sensors. The copter state includes:
* Angle rate of the copter – pitch_rate, roll_rate, yaw_rate;
* Copter orientation (in the local coordinate system) – pitch, roll, yaw (one of presentations);
* Angle rate of the copter – roll_rate, pitch_rate, yaw_rate;
* Copter orientation (in the local coordinate system) – roll, pitch, yaw (one of presentations);
* Copter position (in the local coordinate system) – x, y, z;
* Copter speed (in the local coordinate system) – vx, vy, vz;
* Global coordinates of the copter – latitude, longitude, altitude;

12
docs/en/simple_offboard.md
View File

@@ -51,11 +51,11 @@ Response format:
* `lat, lon` – drone latitude and longitude *(degrees)*, requires [GPS](gps.md) module;
* `alt` – altitude in the global coordinate system (according to [WGS-84](https://ru.wikipedia.org/wiki/WGS_84) standard, not <abbr title="Above Mean Sea Level">AMSL</abbr>!), requires [GPS](gps.md) module;
* `vx, vy, vz` – drone velocity *(m/s)*;
* `pitch` – pitch angle *(radians)*;
* `roll` – roll angle *(radians)*;
* `pitch` – pitch angle *(radians)*;
* `yaw` — yaw angle *(radians)*;
* `pitch_rate` — angular pitch velocity *(rad/s)*;
* `roll_rate` – angular roll velocity *(rad/s)*;
* `pitch_rate` — angular pitch velocity *(rad/s)*;
* `yaw_rate` – angular yaw velocity *(rad/s)*;
* `voltage` – total battery voltage *(V)*;
* `cell_voltage` – battery cell voltage *(V)*.
@@ -261,22 +261,22 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
### set_attitude
Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
Set roll, pitch, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
Parameters:
* `pitch`, `roll`, `yaw` – requested pitch, roll, and yaw angle *(radians)*;
* `roll`, `pitch`, `yaw` – requested roll, pitch, and yaw angle *(radians)*;
* `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
* `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
* `frame_id` – [coordinate system](frames.md) for `yaw` (Default value: `map`).
### set_rates
Set pitch, roll, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
Set roll, pitch, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
Parameters:
* `pitch_rate`, `roll_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
* `roll_rate`, `pitch_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
* `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
* `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);

8
docs/en/snippets.md
View File

@@ -144,7 +144,7 @@ Determine whether the copter is turned upside-down:
PI_2 = math.pi / 2
telem = get_telemetry()
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
```
### # {#angle-hor}
@@ -155,8 +155,8 @@ Calculate the copter horizontal angle:
PI_2 = math.pi / 2
telem = get_telemetry()
flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
angle_to_horizon = math.atan(math.hypot(math.tan(telem.roll), math.tan(telem.pitch)))
if flipped:
angle_to_horizon = math.pi - angle_to_horizon
```
@@ -324,7 +324,7 @@ def flip():
while True:
telem = get_telemetry()
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
if flipped:
break

4
docs/ru/parameters.md
View File

@@ -60,8 +60,8 @@
Estimator это подсистема, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит:
* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate;
* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений);
* угловая скорость коптера – roll_rate, pitch_rate, yaw_rate;
* ориентация коптера (в локальной системе координат) – roll (крен), pitch (тангаж), yaw (рысканье) (одно из представлений);
* позиция коптера (в локальной системе координат) – x, y, z;
* скорость коптера (в локальной системе координат) – vx, vy, vz;
* глобальные координаты коптера – latitude, longitude, altitude;

8
docs/ru/simple_offboard.md
View File

@@ -51,11 +51,11 @@ land = rospy.ServiceProxy('land', Trigger)
* `lat, lon` – широта, долгота *(градусы)*, необходимо наличие [GPS](gps.md);
* `alt` – высота в глобальной системе координат (стандарт [WGS-84](https://ru.wikipedia.org/wiki/WGS_84), не <abbr title="Above Mean Sea Level, выше среднего уровня моря">AMSL</abbr>!), необходимо наличие [GPS](gps.md);
* `vx, vy, vz` – скорость коптера *(м/с)*;
* `pitch` – угол по тангажу *(радианы)*;
* `roll` – угол по крену *(радианы)*;
* `pitch` – угол по тангажу *(радианы)*;
* `yaw` – угол по рысканью *(радианы)*;
* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
* `roll_rate` – угловая скорость по крену *(рад/с)*;
* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
* `yaw_rate` – угловая скорость по рысканью *(рад/с)*;
* `voltage` – общее напряжение аккумулятора *(В)*;
* `cell_voltage` – напряжение аккумулятора на ячейку *(В)*.
@@ -265,7 +265,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
Параметры:
* `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
* `roll`, `pitch`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
* `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ);
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
* `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`).
@@ -276,7 +276,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
Параметры:
* `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
* `roll_rate`, `pitch_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
* `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ).
* `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);

6
docs/ru/snippets.md
View File

@@ -154,7 +154,7 @@ new_pose = tf_buffer.transform(pose, frame_id, transform_timeout)
PI_2 = math.pi / 2
telem = get_telemetry()
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
```
### # {#angle-hor}
@@ -165,7 +165,7 @@ flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
PI_2 = math.pi / 2
telem = get_telemetry()
flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
if flipped:
angle_to_horizon = math.pi - angle_to_horizon
@@ -335,7 +335,7 @@ def flip():
while True:
telem = get_telemetry()
flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
if flipped:
break