Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

before gps test

This commit is contained in:
franchioping 2026-03-25 18:26:56 +00:00
parent 36ea82a13c
commit 8c087ab5f3
4 changed files with 43 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
main/CMakeLists.txt
View File

@ -1,4 +1,6 @@
idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp" file(GLOB_RECURSE SOURCES "*.cpp")
idf_component_register(SRCS "${SOURCES}"
INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT) INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT)
target_compile_options(${COMPONENT_LIB} PRIVATE target_compile_options(${COMPONENT_LIB} PRIVATE

4
main/gps.h
View File

@ -1,12 +1,14 @@
#pragma once #pragma once
#include "HardwareSerial.h"
#include "Wire.h" #include "Wire.h"
#include <Adafruit_GPS.h> #include <Adafruit_GPS.h>
struct GPS { struct GPS {
Adafruit_GPS gps; Adafruit_GPS gps;
GPS(TwoWire *theWire) { GPS(TwoWire *theWire) {
gps = Adafruit_GPS(theWire); Serial2.begin(9600);
gps = Adafruit_GPS(&Serial2);
gps.begin(0x10); gps.begin(0x10);
gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);

14
main/main.cpp
View File

@ -10,27 +10,33 @@
#include "env_sens.h" #include "env_sens.h"
static const constexpr char *TAG = "Main"; static const constexpr char *TAG = "Main";
const gpio_num_t MOTOR_PIN = GPIO_NUM_16; const gpio_num_t MOTOR_PIN = GPIO_NUM_22;
DShotRMT motor(MOTOR_PIN, DSHOT300, false); DShotRMT motor(MOTOR_PIN, DSHOT300, true);
extern "C" void app_main(void) { extern "C" void app_main(void) {
initArduino(); initArduino();
motor.begin(); printf("beg: %d", motor.begin().result_code);
ESP_LOGI(TAG, "Arming ESC..."); ESP_LOGI(TAG, "Arming ESC...");
// Send 0 throttle for 4 seconds to arm // Send 0 throttle for 4 seconds to arm
unsigned long armTime = millis(); unsigned long armTime = millis();
while (millis() - armTime < 4000) { while (millis() - armTime < 4000) {
motor.sendThrottlePercent(0); motor.sendThrottlePercent(0);
// delay(1);
} }
delay(50);
ESP_LOGI(TAG, "Motor Armed. Ramping up..."); ESP_LOGI(TAG, "Motor Armed. Ramping up...");
// Send 10% throttle for 5 seconds // Send 10% throttle for 5 seconds
unsigned long runTime = millis(); unsigned long runTime = millis();
while (millis() - runTime < 5000) { while (millis() - runTime < 5000) {
auto res = motor.sendThrottlePercent(10); auto res = motor.sendThrottlePercent(10);
// printf("%d, %d\n", res.result_code, res.erpm); printf("%d, %d\n", res.result_code, res.erpm);
//
// ESP_LOGI(TAG, "current: %d",
// motor.getTelemetry().telemetry_data.current);
// delay(1);
} }
ESP_LOGI(TAG, "Stopping motor"); ESP_LOGI(TAG, "Stopping motor");

27
main/pos.cpp Normal file
View File

@ -0,0 +1,27 @@
#include <Eigen/Dense>
struct nav_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.01f);
Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.01f);
void predict(float dt, Eigen::Vector3f accel) {
Eigen::Vector3f next_velocity = this->velocity + (accel * dt);
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->velocity = next_velocity;
}
void measure(Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
this->position = (Eigen::Vector3f::Ones() - gps_weight_pos)
.cwiseProduct(this->position) +
gps_weight_pos.cwiseProduct(gps_pos);
this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel)
.cwiseProduct(this->velocity) +
gps_weight_vel.cwiseProduct(gps_vel);
}
};