Skip to content

Commit

Permalink
Improve multiple points
Browse files Browse the repository at this point in the history
- Add CRC
- Format code
- Remove some unused stuff

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
  • Loading branch information
patrickelectric committed Mar 19, 2024
1 parent 741d95c commit 7f01efd
Show file tree
Hide file tree
Showing 4 changed files with 137 additions and 89 deletions.
1 change: 0 additions & 1 deletion Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@ indicatif = "0.17"
mavlink = { version = "0.10.0", features = [ "ardupilotmega", "emit-extensions"] }
num-derive = "0.3"
num-traits = "0.2"
sha-1 = "0.10.1"
structopt = "0.3"
strum = "0.21"
strum_macros = "0.21"
118 changes: 73 additions & 45 deletions src/controller.rs
Original file line number Diff line number Diff line change
@@ -1,22 +1,21 @@
use std::io::Write;
use std::process::exit;
use std::time::{Duration, SystemTime};
use std::{io::Write, path::PathBuf};
use std::time::SystemTime;

use crate::mavftp::*;
use num_traits::FromPrimitive;

use indicatif::{ProgressBar, ProgressState, ProgressStyle};
use sha1::{Digest, Sha1};

use std::fs::OpenOptions;
use std::io::{Seek, SeekFrom};
use std::io::{Read, Seek, SeekFrom};

enum OperationStatus {
ScanningFolder(ScanningFolderStatus),
OpeningFile(OpeningFileStatus),
ReadingFile(ReadingFileStatus),
Reset,
CalcFileCRC32(CalcFileCRC32Status)
CalcFileCRC32(CalcFileCRC32Status),
}

struct ScanningFolderStatus {
Expand Down Expand Up @@ -80,47 +79,38 @@ impl Controller {
}

pub fn run(&mut self) -> Option<MavlinkFtpPayload> {
/*
if self.last_time.elapsed().unwrap() > Duration::from_millis(2) {
self.last_time = SystemTime::now();
self.waiting = false;
}
*/
if self.waiting {
return None;
}
self.waiting = true;
match &self.status {
Some(OperationStatus::Reset) => {
return Some(MavlinkFtpPayload::newResetSesions(
1,
self.session,
));
return Some(MavlinkFtpPayload::new_reset_sesions(1, self.session));
}
Some(OperationStatus::ScanningFolder(status)) => {
return Some(MavlinkFtpPayload::newListDirectory(
return Some(MavlinkFtpPayload::new_list_directory(
1,
self.session,
status.offset as u32,
&status.path,
));
}
Some(OperationStatus::OpeningFile(status)) => {
return Some(MavlinkFtpPayload::newOpenFile(
return Some(MavlinkFtpPayload::new_open_file(
1,
self.session,
&status.path,
));
}
Some(OperationStatus::CalcFileCRC32(status)) => {
return Some(MavlinkFtpPayload::newCalcFileCRC32(
return Some(MavlinkFtpPayload::new_calc_file_crc32(
1,
self.session,
&status.path,
));
}
Some(OperationStatus::ReadingFile(status)) => {
return Some(MavlinkFtpPayload::newReadFile(
return Some(MavlinkFtpPayload::new_read_file(
1,
self.session,
0,
Expand All @@ -136,15 +126,9 @@ impl Controller {
message: &mavlink::common::FILE_TRANSFER_PROTOCOL_DATA,
) -> Option<mavlink::common::MavMessage> {
self.waiting = false;
let payload = &message.payload;
let opcode = payload[3];

let opcode = MavlinkFtpOpcode::from_u8(opcode).unwrap();

match opcode {
let payload = MavlinkFtpPayload::from_bytes(&message.payload).unwrap();
match payload.opcode {
MavlinkFtpOpcode::Ack => {
let payload = MavlinkFtpPayload::from_bytes(&payload).unwrap();

match &mut self.status {
Some(OperationStatus::Reset) => {
if payload.req_opcode == MavlinkFtpOpcode::ResetSessions {
Expand Down Expand Up @@ -180,12 +164,13 @@ impl Controller {
target_network: 0,
target_system: 1,
target_component: 1,
payload: MavlinkFtpPayload::newListDirectory(
payload: MavlinkFtpPayload::new_list_directory(
1,
self.session,
status.offset as u32,
&status.path,
).to_bytes(),
)
.to_bytes(),
},
));
}
Expand Down Expand Up @@ -217,7 +202,8 @@ impl Controller {
file: OpenOptions::new()
.write(true)
.create(true)
.open("/tmp/potato2").unwrap(),
.open("/tmp/potato2")
.unwrap(),
}));

return None;
Expand All @@ -233,10 +219,13 @@ impl Controller {
println!("0x{:x?}", crc);
exit(0);
}
},
}
Some(OperationStatus::ReadingFile(status)) => {
let chunk = &payload.data;
status.file.seek(SeekFrom::Start(payload.offset.into())).unwrap();
status
.file
.seek(SeekFrom::Start(payload.offset.into()))
.unwrap();
status.file.write_all(chunk).unwrap();
status.offset = payload.offset + payload.size as u32;
if let Some(progress) = &self.progress {
Expand All @@ -250,12 +239,13 @@ impl Controller {
target_network: 0,
target_system: 1,
target_component: 1,
payload: MavlinkFtpPayload::newReadFile(
payload: MavlinkFtpPayload::new_read_file(
payload.seq_number + 1,
self.session,
status.offset,
usize::MAX,
).to_bytes(),
)
.to_bytes(),
},
));
}
Expand Down Expand Up @@ -283,30 +273,54 @@ impl Controller {
));
*/
} else {
//std::io::stdout().write_all(&status.content).unwrap();
if let Some(progress) = &self.progress {
progress.finish();
}

self.waiting = false;
dbg!("Done!!");
//let mut hasher = Sha1::new();
//dbg!(&status.content.len());
//hasher.update(&status.content);
//println!("{:x?}", hasher.finalize());
//let mut f = std::fs::File::create("/tmp/potato").ok().unwrap();
//f.write_all(&status.content);
self.status = None;

// Lets get the crc
let mut buffer = Vec::new();
let mut file = std::fs::File::open("/tmp/potato2").unwrap();
file.read_to_end(&mut buffer).unwrap();
let crc = mavlink_crc32(&buffer);
println!("{:08x}", crc);
return None;
}
}
None => return None,
}
}
MavlinkFtpOpcode::Nak => {
let nak_code = MavlinkFtpNak::from_u8(payload[12]).unwrap();
let nak_code = MavlinkFtpNak::from_u8(payload.data[0]).unwrap();

match nak_code {
MavlinkFtpNak::EOF => {
exit(0);
// We finished the current operation
dbg!(&self.entries);
match &payload.req_opcode {
MavlinkFtpOpcode::ListDirectory => {
println!("{:<4} {:<30} {:<10}", "Type", "Name", "Size");
println!("{}", "-".repeat(40));
self.entries
.sort_by(|a, b| a.name.partial_cmp(&b.name).unwrap());
for entry in &self.entries {
let item_type = match entry.entry_type {
EntryType::File => 'F',
EntryType::Directory => 'D',
EntryType::Skip => 'S',
};
println!(
"{:<4} {:<30} {:<10}",
item_type,
entry.name,
format_size(entry.size as u64)
);
}
}
_ => {}
}
exit(0);
self.status = None;
return None;
}
Expand All @@ -325,3 +339,17 @@ impl Controller {
return None;
}
}

fn format_size(size: u64) -> String {
const KILO: u64 = 1024;
const MEGA: u64 = KILO * 1024;
const GIGA: u64 = MEGA * 1024;

match size {
0 => String::new(),
1..=KILO => format!("{} B", size),
KILO..=MEGA => format!("{:.1} KB", (size as f64) / (KILO as f64)),
MEGA..=GIGA => format!("{:.1} MB", (size as f64) / (MEGA as f64)),
_ => format!("{:.1} GB", (size as f64) / (GIGA as f64)),
}
}
1 change: 0 additions & 1 deletion src/main.rs
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
mod mavftp;
use mavftp::*;

mod controller;
use controller::*;
Expand Down
Loading

0 comments on commit 7f01efd

Please sign in to comment.