247 lines
6.2 KiB
C
247 lines
6.2 KiB
C
/*
|
|
* Copyright (C) 2023 Daniele Lacamera <root@danielinux.net>
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Lesser General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <unicore-mx/stm32/memorymap.h>
|
|
#include <unicore-mx/stm32/rcc.h>
|
|
#include <unicore-mx/stm32/gpio.h>
|
|
#include <unicore-mx/stm32/usart.h>
|
|
#include <unicore-mx/stm32/f7/nvic.h>
|
|
#include <stddef.h>
|
|
#include <string.h>
|
|
#include "systick.h"
|
|
#include "task.h"
|
|
#include "ksp-serial.h"
|
|
|
|
static void ksp_serial_poll(uint32_t ev, void *arg);
|
|
static void ksp_serial_init(void);
|
|
static uint32_t last_poll = 0;
|
|
static volatile uint8_t buf[4096]; /* Reserve a page for serial comm */
|
|
static volatile uint32_t buf_count = 0;
|
|
static uint8_t kspio_rxlen;
|
|
static int kspio_busy;
|
|
|
|
|
|
|
|
#define KSP_UART USART1
|
|
|
|
const char ksp_name[] = "KSPSerial";
|
|
|
|
struct task ksp_task = {
|
|
.init = ksp_serial_init,
|
|
.run = ksp_serial_poll,
|
|
.events = EV_SERIAL,
|
|
.name = ksp_name
|
|
};
|
|
|
|
|
|
static uint8_t cur_vdata_buf[204];
|
|
vesselData_t *cur_vdata = (vesselData_t *)(cur_vdata_buf + 3);
|
|
|
|
|
|
const char vesselDataName[][18] = {
|
|
"id", //1
|
|
"AP", //2
|
|
"PE", //3
|
|
"SemiMajorAxis", //4
|
|
"SemiMinorAxis", //5
|
|
"VVI", //6
|
|
"e", //7
|
|
"inc", //8
|
|
"G", //9
|
|
"TAp", //10
|
|
"TPe", //11
|
|
"TrueAnomaly", //12
|
|
"Density", //13
|
|
"t period", //14
|
|
"RAlt", //15
|
|
"Alt", //16
|
|
"Vsurf", //17
|
|
"Lat", //18
|
|
"Lon", //19
|
|
"LiquidFuelTot", //20
|
|
"LiquidFuel", //21
|
|
"OxidizerTot", //22
|
|
"Oxidizer", //23
|
|
"EChargeTot", //24
|
|
"ECharge", //25
|
|
"MonoPropTot", //26
|
|
"MonoProp", //27
|
|
"IntakeAirTot", //28
|
|
"IntakeAir", //29
|
|
"SolidFuelTot", //30
|
|
"SolidFuel", //31
|
|
"XenonGasTot", //32
|
|
"XenonGas", //33
|
|
"LiquidFuelTotS", //34
|
|
"LiquidFuelS", //35
|
|
"OxidizerTotS", //36
|
|
"OxidizerS", //37
|
|
"MissionTime", //38
|
|
"deltaTime", //39
|
|
"VOrbit", //40
|
|
"MNTime", //41
|
|
"MNDeltaV", //42
|
|
"Pitch", //43
|
|
"Roll", //44
|
|
"Heading", //45
|
|
};
|
|
|
|
uint8_t ksp_serial_checksum(uint8_t *bytes, uint8_t sz)
|
|
{
|
|
uint8_t chk = sz;
|
|
uint8_t i;
|
|
for (i = 0; i < sz; i++) {
|
|
chk ^= bytes[i];
|
|
}
|
|
return chk;
|
|
}
|
|
|
|
|
|
void ksp_serial_init(void)
|
|
{
|
|
last_poll = jiffies;
|
|
rcc_periph_clock_enable(RCC_USART1);
|
|
rcc_periph_clock_enable(RCC_GPIOA);
|
|
rcc_periph_clock_enable(RCC_GPIOB);
|
|
|
|
gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9);
|
|
gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO7);
|
|
gpio_set_af(GPIOA, GPIO_AF7, GPIO9);
|
|
gpio_set_af(GPIOB, GPIO_AF7, GPIO7);
|
|
|
|
|
|
/* Set up USART/UART parameters using the unicore-mx helper functions */
|
|
usart_enable_rx_interrupt(KSP_UART);
|
|
usart_set_baudrate(KSP_UART, 115200);
|
|
usart_set_databits(KSP_UART, 8);
|
|
usart_set_stopbits(KSP_UART, USART_STOPBITS_1);
|
|
usart_set_mode(KSP_UART, USART_MODE_TX_RX);
|
|
usart_set_parity(KSP_UART, USART_PARITY_NONE);
|
|
usart_set_flow_control(KSP_UART, USART_FLOWCONTROL_NONE);
|
|
usart_enable_rx_interrupt(KSP_UART);
|
|
USART_CR1(KSP_UART) &= ~(USART_CR1_TCIE);
|
|
nvic_enable_irq(NVIC_USART1_IRQ);
|
|
usart_enable(KSP_UART);
|
|
}
|
|
|
|
void kspserial_setup(void)
|
|
{
|
|
register_task(&ksp_task);
|
|
}
|
|
|
|
void ksp_serial_send(const void *data, uint8_t len)
|
|
{
|
|
int i;
|
|
unsigned char *bytes = (unsigned char *)data;
|
|
uint8_t chk = ksp_serial_checksum(bytes, len);
|
|
usart_send_blocking(KSP_UART, 0xBE);
|
|
usart_send_blocking(KSP_UART, 0xEF);
|
|
usart_send_blocking(KSP_UART, len);
|
|
for(i = 0; i < len; i++)
|
|
usart_send_blocking(KSP_UART, bytes[i]);
|
|
usart_send_blocking(KSP_UART, chk);
|
|
}
|
|
|
|
static void ksp_serial_handshake(void)
|
|
{
|
|
handShakePacket_t hs;
|
|
hs.id = 0;
|
|
hs.M1 = 3;
|
|
hs.M2 = 1;
|
|
hs.M3 = 4;
|
|
ksp_serial_send(&hs, sizeof(hs));
|
|
}
|
|
|
|
static void ksp_serial_rxdata(void)
|
|
{
|
|
uint8_t chk_exp, chk_rx;
|
|
if (buf[2] == sizeof(vesselData_t)) {
|
|
chk_rx = buf[sizeof(vesselData_t) + 3];
|
|
chk_exp = ksp_serial_checksum(buf + 3, buf[2]);
|
|
if (chk_rx != chk_exp) {
|
|
return;
|
|
}
|
|
memcpy(cur_vdata_buf, buf, buf[2] + 3);
|
|
memset(buf, 0, sizeof(buf));
|
|
buf_count = 0;
|
|
trigger_event(EV_VESSELDATA);
|
|
}
|
|
}
|
|
|
|
static void ksp_serial_poll(uint32_t ev, void *arg)
|
|
{
|
|
if (buf_count == 0) {
|
|
clear_event(EV_SERIAL);
|
|
return;
|
|
}
|
|
|
|
while((buf_count < (buf[2] + 4)) && usart_is_recv_ready(KSP_UART)) {
|
|
buf[buf_count++] = usart_recv(KSP_UART);
|
|
}
|
|
if (buf_count < buf[2] + 4)
|
|
return;
|
|
|
|
if (buf[3] == (uint8_t)0) {
|
|
ksp_serial_handshake();
|
|
}
|
|
else if (buf[3] == (uint8_t)1) {
|
|
ksp_serial_rxdata();
|
|
}
|
|
clear_event(EV_SERIAL);
|
|
buf_count = 0;
|
|
usart_enable_rx_interrupt(KSP_UART);
|
|
kspio_busy = 0;
|
|
last_poll = jiffies;
|
|
}
|
|
|
|
#include <unicore-mx/stm32/usart.h>
|
|
#include "task.h"
|
|
void isr_usart1(void)
|
|
{
|
|
uint16_t rx;
|
|
usart_clear_rx_interrupt(KSP_UART);
|
|
usart_clear_tx_interrupt(KSP_UART);
|
|
|
|
if (usart_is_recv_ready(KSP_UART)) {
|
|
rx = usart_recv(KSP_UART);
|
|
buf[buf_count++] = (unsigned char)(rx & 0xFF);
|
|
if (kspio_busy) {
|
|
trigger_event(EV_SERIAL);
|
|
return;
|
|
}
|
|
|
|
if ((buf_count == 1) && (buf[0] != 0xBE)) {
|
|
buf_count = 0;
|
|
return;
|
|
}
|
|
|
|
if ((buf_count == 2) && (buf[1] != 0xEF)) {
|
|
buf_count = 0;
|
|
return;
|
|
}
|
|
|
|
if (buf_count == 3) {
|
|
kspio_rxlen = buf[2];
|
|
kspio_busy++;
|
|
trigger_event(EV_SERIAL);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|