zhig02/Proto_RTIC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Getting and extraction received proto message with quaternion

Browse Source
This commit is contained in:
Игорь Жуков 2024-12-16 22:11:57 +03:00
parent aae8f12a56
commit 81dd42c931
5 changed files with 273 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -1,2 +1,3 @@
/target
Cargo.lock
Cargo.lock
__pycache__

15
.vscode/launch.json vendored Normal file
View File

@ -0,0 +1,15 @@
{
"version": "0.2.0",
"configurations": [
{
"name": "Cortex Debug",
"cwd": "${workspaceFolder}",
"executable": "./target/thumbv7em-none-eabihf/debug/MRTIC",
"request": "launch",
"type": "cortex-debug",
"runToEntryPoint": "main",
"servertype": "stlink"
}
]
}

240
src/main.rs
View File

@ -15,39 +15,37 @@ mod proto {
#![allow(nonstandard_style, unused, irrefutable_let_patterns)]
include!(concat!(env!("OUT_DIR"), "/meta.rs"));
}
//use proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_;
use proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::*;
//use proto::example_package_::*;
#[rtic::app(device = stm32f7xx_hal::pac, peripherals = true, dispatchers = [EXTI0])]
mod app {
use proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta;
use shared_resources::message_buffer_that_needs_to_be_locked;
//Declare Parameters
use stm32f7xx_hal::{
gpio::{GpioExt, Output, PushPull, PI1},
pac::{Interrupt, USART1},
prelude::*,
rcc::RccExt,
serial::{Config, Event, Rx, Serial, Tx}
};
use super::*;
use core::{fmt::Write, mem::size_of};
use rtic_monotonics::systick::prelude::*;
use embedded_alloc::LlffHeap as Heap;
use ringbuffer::{AllocRingBuffer, RingBuffer};
use micropb::{PbRead, PbEncoder, PbDecoder, MessageDecode, MessageEncode};
use alloc::string::String;
//Declare Parameters
use super::*;
use embedded_alloc::LlffHeap as Heap;
use micropb::{MessageDecode, MessageEncode, PbDecoder};
use ringbuffer::{AllocRingBuffer, RingBuffer};
use rtic_monotonics::systick::prelude::*;
use stm32f7xx_hal::{
gpio::{GpioExt, Output, PushPull, PI1},
pac::USART1,
prelude::*,
rcc::RccExt,
serial::{Config, Event, Rx, Serial, Tx},
};
use alloc::vec::Vec;
systick_monotonic!(Mono, 1000);
#[global_allocator]
static HEAP: Heap = Heap::empty();
#[shared]
struct Shared {
message_buffer: AllocRingBuffer<u8>
message_buffer: AllocRingBuffer<u8>,
}
#[local]
@ -61,13 +59,12 @@ mod app {
//Init Function
#[init]
fn init(ctx: init::Context) -> (Shared, Local) {
//Allocator
{
use core::mem::MaybeUninit;
const HEAP_SIZE: usize = 2048;
static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) }
{
use core::mem::MaybeUninit;
const HEAP_SIZE: usize = 2048;
static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) }
}
// Cortex-M peripherals
@ -76,62 +73,58 @@ mod app {
//Systick
Mono::start(ctx.core.SYST, 36_000_000);
//GPIO
let GPIOI = ctx.device.GPIOI.split();
let GPIOA = ctx.device.GPIOA.split();
let GPIOB = ctx.device.GPIOB.split();
//UART
let usart1_tx = GPIOA.pa9.into_alternate();
let usart1_rx = GPIOB.pb7.into_alternate();
let usart1_config = Config {
baud_rate: 9600.bps(),
..Default::default()
};
let mut usart1 =
Serial::new(
ctx.device.USART1,
(usart1_tx, usart1_rx),
let mut usart1 = Serial::new(
ctx.device.USART1,
(usart1_tx, usart1_rx),
&clocks,
usart1_config);
usart1_config,
);
usart1.listen(Event::Rxne);
let (tx_uart1, rx_uart1) = usart1.split();
//LED
let mut ld1 = GPIOI
.pi1
.into_push_pull_output();
let mut ld1 = GPIOI.pi1.into_push_pull_output();
ld1.set_high();
let read_buf_serial: AllocRingBuffer<u8> = AllocRingBuffer::new(128);
//TASKS
blinker::spawn().ok();
//serial_task::spawn().ok();
proto_task::spawn().ok();
let mut stream: Vec<u8> = Vec::new();
//let stream: Vec<u8> = Vec::new();
// let mut decoder = PbDecoder::new(stream.as_slice());
// let mut meta_out: ProtoMeta = Default::default();
// meta_out.decode_len_delimited(&mut decoder);
defmt::info!("INITED");
(
Shared {
message_buffer: read_buf_serial
},
message_buffer: read_buf_serial,
},
Local {
led: ld1,
state : true,
led: ld1,
state: true,
tx_vcp: tx_uart1,
rx_vcp: rx_uart1
}
rx_vcp: rx_uart1,
},
)
}
@ -150,59 +143,51 @@ mod app {
}
#[task(local = [led, state], priority = 1)]
async fn blinker(ctx: blinker::Context){
loop{
if *ctx.local.state{
async fn blinker(ctx: blinker::Context) {
loop {
if *ctx.local.state {
ctx.local.led.set_low();
*ctx.local.state = false;
}
else{
} else {
ctx.local.led.set_high();
*ctx.local.state = true;
}
Mono::delay(100.millis()).await;
}
}
#[task(local = [tx_vcp], priority = 1)]
async fn serial_task(ctx: serial_task::Context){
defmt::info!("Start sending data");
loop{
// #[task(local = [tx_vcp], priority = 1)]
// async fn serial_task(ctx: serial_task::Context){
// defmt::info!("Start sending data");
// loop{
ctx.local.tx_vcp.write_str("Hello\n").unwrap();
// ctx.local.tx_vcp.write_str("Hello\n\r").unwrap();
Mono::delay(1000.millis()).await;
}
}
// Mono::delay(2000.millis()).await;
// }
// }
#[task(binds = USART1, local = [rx_vcp], shared = [message_buffer])]
fn usart1_handler(ctx: usart1_handler::Context){
fn usart1_handler(ctx: usart1_handler::Context) {
let serial = ctx.local.rx_vcp;
let mut message_buffer = ctx.shared.message_buffer;
if let Ok(byte) = serial.read() {
message_buffer.lock(|buf| {
buf.push(byte);
});
message_buffer.lock(|buf| {
buf.push(byte);
});
}
}
#[task(shared = [message_buffer], priority = 1)]
async fn proto_task(ctx: proto_task::Context)
{
#[task(shared = [message_buffer], local = [tx_vcp], priority = 1)]
async fn proto_task(ctx: proto_task::Context) {
let mut message_buffer = ctx.shared.message_buffer;
let mut buffer = Vec::new();
let mut meta_out:ProtoMeta = Default::default();
loop{
let mut meta_out: ProtoMeta = Default::default();
loop {
let mut is_empty = false;
message_buffer.lock(|buf| {
@ -215,20 +200,91 @@ mod app {
}
}
});
if is_empty {
// Sleep for a while if the buffer is empty
Mono::delay(5000.millis()).await;
defmt::trace!("await for data");
continue;
}
let mut decoder = PbDecoder::new(buffer.as_slice());
if let Ok(message) = meta_out.decode_len_delimited(&mut decoder) {
defmt::info!("Received message: {:?}", message);
} else {
defmt::info!("Failed to parse message");
}
defmt::info!("Buffer content: {:?}", buffer.as_slice());
let mut decoder = PbDecoder::new(buffer.as_slice());
meta_out
.decode(&mut decoder, buffer.len())
.expect("decoding failed");
let szmsg = meta_out.compute_size();
defmt::info!("SIZE OF MSG = {:?}", szmsg);
if let Ok(message) = meta_out.decode(&mut decoder, buffer.len()) {
defmt::info!("Received message: {:?}", message);
} else {
defmt::error!("Failed to parse message");
}
// let mut encoder = PbEncoder::new(Vec::new());
// meta_out.encode(&mut encoder);
// let data_out = encoder.into_writer();
// for &byte in &data_out {
// if let Err(e) = ctx.local.tx_vcp.write(byte) {
// defmt::error!("Failed to send byte");
// break;
// }
// Mono::delay(1.millis()).await;
// }
// if let Err(e) = ctx.local.tx_vcp.flush() {
// defmt::error!("Failed to flush TX buffer");
// }
// defmt::info!("DATA SENDED");
struct Quaternion {
x: f32,
y: f32,
z: f32,
w: f32,
}
let quat_meta = &meta_out.items.get("quaternion").unwrap();
let value = quat_meta.protoValue.value.as_ref().unwrap(); //Options
match value {
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::StringValue(_) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::BooleanValue(_) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::DoubleValue(_) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::FloatValue(_) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::Int32Value(_) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::Int64Value(_) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::BytesValue(_vec) => todo!(),
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::ListValue(proto_value_list) => {
let floats: Vec<f32> = proto_value_list.values.iter().filter_map(|v| {
if let Some(proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::FloatValue(f)) = &v.value {
Some(*f)
} else {
None
}
}).collect();
if floats.len() == 4 {
let quaternion = Quaternion {
w: floats[0],
x: floats[1],
y: floats[2],
z: floats[3],
};
defmt::info!(
"Quaternion : w = {}, x = {}, y = {}, z = {}",
quaternion.w,
quaternion.x,
quaternion.y,
quaternion.z
);
}
},
proto::space_::kscience_::dataforge_::io_::proto_::ProtoMeta_::ProtoValue_::Value::Float64ListValue(_float64_list) => todo!(),
}
}
}
//APP ENDS

106
src/proto/backforward.py Normal file
View File

@ -0,0 +1,106 @@
import meta_pb2
import numpy as np
import serial
import time
def create_quaternion():
# Constructing the ProtoMeta message with two ProtoValue entries
quaternion = meta_pb2.ProtoMeta(
protoValue=None,
items={
'quaternion': meta_pb2.ProtoMeta(
protoValue=meta_pb2.ProtoMeta.ProtoValue(
listValue=meta_pb2.ProtoMeta.ProtoValueList(
values=[
meta_pb2.ProtoMeta.ProtoValue(floatValue=np.float32(1.0)),
meta_pb2.ProtoMeta.ProtoValue(floatValue=np.float32(0.0)),
meta_pb2.ProtoMeta.ProtoValue(floatValue=np.float32(0.0)),
meta_pb2.ProtoMeta.ProtoValue(floatValue=np.float32(0.0))
]
)
)
)
}
)
return quaternion
def send_message(serial_port, message):
# Serialize the message to a byte string
serialized_message = message.SerializeToString()
# Optionally, add length-prefix framing (2 bytes, little endian)
msg_length = len(serialized_message)
framed_message = msg_length.to_bytes(2, byteorder='little') + serialized_message
# Send the framed message
serial_port.write(framed_message)
print("Message sent over COM3")
def main():
# Initialize serial port
try:
ser = serial.Serial('COM3', 9600, timeout=1)
print("Opened COM3 successfully.")
except serial.SerialException as e:
print(f"Error opening COM3: {e}")
return
# Create the quaternion message
quaternion = create_quaternion()
# Serialize and send the message
send_message(ser, quaternion)
# Optionally, wait for a moment before starting to receive
time.sleep(1)
# Start receiving messages indefinitely
receive_messages(ser)
def receive_messages(serial_port):
print("Starting to receive messages...")
buffer = bytearray()
while True:
try:
# Read incoming bytes
incoming = serial_port.read(serial_port.in_waiting or 1)
if incoming:
print(incoming)
buffer.extend(incoming)
while True:
if len(buffer) < 2:
# Not enough data to determine message length
break
# Extract message length (2 bytes, little endian)
msg_length = int.from_bytes(buffer[:2], byteorder='little')
if len(buffer) < 2 + msg_length:
# Full message not yet received
break
# Extract the full message
msg_bytes = buffer[2:2 + msg_length]
buffer = buffer[2 + msg_length:] # Remove the processed message from buffer
# Deserialize the message
received_quaternion = meta_pb2.ProtoMeta()
received_quaternion.ParseFromString(msg_bytes)
# Handle the received message
handle_received_message(received_quaternion)
except serial.SerialException as e:
print(f"Serial error: {e}")
break
except Exception as e:
print(f"Unexpected error: {e}")
break
def handle_received_message(message):
# Implement your logic to handle the received ProtoMeta message
print("\nReceived ProtoMeta Message:")
print(message)
if __name__ == "__main__":
main()

3
src/proto/simplified_quat.py
View File

@ -23,7 +23,8 @@ quaternion = meta_pb2.ProtoMeta(
# Serialize the message
serialized_message = quaternion.SerializeToString()
dec_message = ' '.join(f'{byte}' for byte in serialized_message)
print(f"\nserialized message (dec): {dec_message}\n")
# Send the serialized message over COM3
with serial.Serial('COM3', 9600, timeout=1) as ser:
ser.write(serialized_message)