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

Significant update to simple_offboard node

Browse Source 
* Allow using nans for most of services parameters
* Add terrain frame
* Remove yaw_rate parameter from most services
* Add set_yaw and set_yaw_rate services
* Correct order for pitch and roll everywhere to match XYZ convention
* Add simple_offboard/state topic
* Add essential tests
* Stop publishing setpoints when land called
This commit is contained in:
Oleg Kalachev
2023-01-12 11:00:05 +03:00
parent 460c3fdbe1
commit 0cfdac43ec
27 changed files with 942 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
clover_blocks/www/blocks.js
View File

@@ -15,6 +15,7 @@ const COLOR_GPIO = 200;
const DOCS_URL = 'https://clover.coex.tech/en/blocks.html';
var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]];
var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]);
function considerFrameId(e) {
if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return;
@@ -22,7 +23,7 @@ function considerFrameId(e) {
var frameId = this.getFieldValue('FRAME_ID');
// set appropriate coordinates labels
if (this.getInput('X')) { // block has x-y-z fields
if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') {
if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') {
this.getInput('X').fieldRow[0].setValue('forward');
this.getInput('Y').fieldRow[0].setValue('left');
this.getInput('Z').fieldRow[0].setValue('up');
@@ -59,8 +60,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');
@@ -73,7 +74,7 @@ function updateSetpointBlock(e) {
Blockly.Blocks['navigate'] = {
init: function () {
let navFrameId = frameIds.slice();
let navFrameId = frameIdsWithTerrain.slice();
navFrameId.push(['global', 'GLOBAL_LOCAL'])
navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
this.appendDummyInput()
@@ -163,14 +164,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")
@@ -213,7 +214,7 @@ Blockly.Blocks['get_position'] = {
.appendField("current")
.appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD")
.appendField("relative to")
.appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID");
.appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID");
this.appendValueInput("ID")
.setCheck("Number")
.appendField("with ID")
@@ -247,7 +248,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).");
@@ -509,7 +510,7 @@ Blockly.Blocks['distance'] = {
.appendField("z");
this.appendDummyInput()
.appendField("relative to")
.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID");
.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID");
this.appendValueInput("ID")
.setCheck("Number")
.appendField("with ID")

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>

7
clover_blocks/www/python.js
View File

@@ -276,10 +276,11 @@ Blockly.Python.angle = function(block) {
}
Blockly.Python.set_yaw = function(block) {
rosDefinitions.setYaw = true;
simpleOffboard();
let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE);
let frameId = buildFrameId(block);
let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`;
let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`;
if (block.getFieldValue('WAIT') == 'TRUE') {
rosDefinitions.waitYaw = true;
simpleOffboard();
@@ -328,11 +329,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`;
}
}