From 211ff6c63ad31e904365354e688ac23ac88b8049 Mon Sep 17 00:00:00 2001 From: Andrey Egorov Date: Tue, 23 Jul 2024 19:02:28 +0300 Subject: [PATCH] Some refactoring. --- api/modem/at/at.go | 9 +- api/modem/gpio.go | 11 +- api/modem/gps.go | 74 ++++++++++ api/modem/modem.go | 361 ++++++++++++++++++++------------------------- main.go | 3 +- 5 files changed, 246 insertions(+), 212 deletions(-) create mode 100644 api/modem/gps.go diff --git a/api/modem/at/at.go b/api/modem/at/at.go index 9e89d1b..6a2f98d 100644 --- a/api/modem/at/at.go +++ b/api/modem/at/at.go @@ -15,6 +15,8 @@ const ( ) type atPort struct { + logger *log.Logger + baudrate int portName string port serial.Port @@ -32,8 +34,9 @@ type Port interface { Send(cmd string) (Resp, error) } -func New(portName string, baudrate int) Port { +func New(logger *log.Logger, portName string, baudrate int) Port { return &atPort{ + logger: logger, portName: portName, baudrate: baudrate, inputBuf: make([]byte, InputBufSize), @@ -49,7 +52,7 @@ func (p *atPort) GetSerialPort() serial.Port { } func (p *atPort) Connect() error { - log.Println("Connecting to", p.portName, "...") + p.logger.Println("Connecting to", p.portName, "...") s, err := serial.Open(p.portName, &serial.Mode{BaudRate: p.baudrate}) if err != nil { return fmt.Errorf("open port: %w", err) @@ -86,7 +89,7 @@ func (p *atPort) makeReq(msg string) (string, error) { } // Read readLen, err := p.port.Read(p.inputBuf) - log.Println(msg, "RAWREAD:", string(p.inputBuf[:readLen])) + p.logger.Println(msg, "RAWREAD:", string(p.inputBuf[:readLen])) if err != nil { return "", fmt.Errorf("port read: %w", err) } diff --git a/api/modem/gpio.go b/api/modem/gpio.go index fec7597..c08da44 100644 --- a/api/modem/gpio.go +++ b/api/modem/gpio.go @@ -8,7 +8,8 @@ import ( ) type gpioPin struct { - Pin gpio.Pin + logger *log.Logger + Pin gpio.Pin } func (p gpioPin) Init() error { @@ -17,22 +18,22 @@ func (p gpioPin) Init() error { func (p gpioPin) sendOnOffSignal() { p.Pin.Output() - log.Println("Power on 0/3 + 100ms") + p.logger.Println("Power on 0/3 + 100ms") p.Pin.Low() p.Pin.Toggle() time.Sleep(100 * time.Millisecond) - log.Println("Power on 1/3 + 3s") + p.logger.Println("Power on 1/3 + 3s") p.Pin.High() p.Pin.Toggle() time.Sleep(3 * time.Second) - log.Println("Power on 2/3 + 30s") + p.logger.Println("Power on 2/3 + 30s") p.Pin.Low() p.Pin.Toggle() time.Sleep(30 * time.Second) - log.Println("Power on 3/3") + p.logger.Println("Power on 3/3") } func (p gpioPin) PowerOn() { diff --git a/api/modem/gps.go b/api/modem/gps.go new file mode 100644 index 0000000..78cdf78 --- /dev/null +++ b/api/modem/gps.go @@ -0,0 +1,74 @@ +package modem + +import ( + "fmt" + "math" + "strconv" + "strings" + "time" +) + +type GpsInfo struct { + Latitude float64 `json:"Latitude"` + Longitude float64 `json:"Longitude"` + LatitudeIndicator string `json:"Latitude_indicator"` // North/South + LongitudeIndicator string `json:"Longitude_indicator"` // West/East + Speed float64 `json:"Speed"` + Course float64 `json:"-"` + Altitude float64 `json:"-"` + Date string `json:"-"` + Time string `json:"-"` +} + +var GpsInfoNil = GpsInfo{} + +func deg2rad(deg float64) float64 { + return deg * (math.Pi / 180) +} + +func (gps *GpsInfo) calculateSpeed(newLatitude, newLongitude float64, lastUpdateTime time.Time) { + earthRad := 6371.0 // TODO ? + dLat := deg2rad(math.Abs(newLatitude - gps.Latitude)) + dLon := deg2rad(math.Abs(newLongitude - gps.Longitude)) + a := math.Sin(dLat/2)*math.Sin(dLat/2) + + math.Cos(deg2rad(newLatitude))*math.Cos(deg2rad(gps.Latitude))*math.Sin(dLon/2)*math.Sin(dLon/2) + c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a)) + + gps.Speed = earthRad * c / (math.Abs(float64(time.Since(lastUpdateTime)))) +} + +func (gps *GpsInfo) decode(str string) error { + var err error + newGpsInfo := GpsInfo{} + strs := strings.Split(strings.Split(strings.Replace(str, " ", "", -1), "\n")[0], ",") + + newGpsInfo.Latitude, err = strconv.ParseFloat(strs[0], 64) + if err != nil { + return fmt.Errorf("parse latitude: %w", err) + } + newGpsInfo.Longitude, err = strconv.ParseFloat(strs[2], 64) + if err != nil { + return fmt.Errorf("parse longitude: %w", err) + } + newGpsInfo.LatitudeIndicator = strs[1] + newGpsInfo.LatitudeIndicator = strs[3] + newGpsInfo.Date = strs[4] + newGpsInfo.Time = strs[5] + newGpsInfo.Altitude, err = strconv.ParseFloat(strs[6], 64) + if err != nil { + return fmt.Errorf("parse altitude: %w", err) + } + newGpsInfo.Speed, err = strconv.ParseFloat(strs[7], 64) + if err != nil { + return fmt.Errorf("parse speed: %w", err) + } + // Course sometimes may be null + if len(strs[8]) > 0 { + newGpsInfo.Course, err = strconv.ParseFloat(strs[8], 64) + if err != nil { + return fmt.Errorf("parse course: %w", err) + } + } + *gps = newGpsInfo + return nil +} diff --git a/api/modem/modem.go b/api/modem/modem.go index a4a6ddc..d38dd81 100644 --- a/api/modem/modem.go +++ b/api/modem/modem.go @@ -2,34 +2,17 @@ package modem import ( "encoding/json" - "errors" "fmt" "log" - "math" "os" "os/exec" "slices" - "strconv" "strings" "time" "github.com/CGSG-2021-AE4/modem-test/api/modem/at" ) -type GpsInfo struct { - Latitude float64 `json:"Latitude"` - Longitude float64 `json:"Longitude"` - LatitudeIndicator string `json:"Latitude_indicator"` // North/South - LongitudeIndicator string `json:"Longitude_indicator"` // West/East - Speed float64 `json:"Speed"` - Course float64 `json:"-"` - Altitude float64 `json:"-"` - Date string `json:"-"` - Time string `json:"-"` -} - -var GpsInfoNil = GpsInfo{} - type modem struct { // Internal values logger *log.Logger @@ -37,9 +20,8 @@ type modem struct { // Serial values baudrate int - deviceName string - port at.Port - isAvailable bool + deviceName string + port at.Port // Gpio values onOffPin gpioPin @@ -51,19 +33,22 @@ type modem struct { type Modem interface { Init() error - SearchPort(isSoft bool) error + // SearchPort(isSoft bool) error Connect() error - Ping() error - SwitchToGpsMode() error - CalculateSpeed(newLatitude, newlongitude float64) + Disconnect() error + IsAvailable() bool + IsConnected() bool + // Ping() error + // SwitchToGpsMode() error + // CalculateSpeed(newLatitude, newlongitude float64) Update() error GetInfo() (string, error) TestGPS() error } -func New() Modem { +func New(logger *log.Logger) Modem { return &modem{ - logger: log.New(os.Stdout, "modem:", log.LstdFlags), + logger: logger, baudrate: 115200, onOffPin: gpioPin{Pin: 6}, lastUpdateTime: time.Now(), @@ -81,17 +66,17 @@ func (m *modem) Init() error { // Search m.logger.Println("=============================== Search") // Soft search - if err := m.SearchPort(true); err != nil { + if err := m.searchPort(true); err != nil { return fmt.Errorf("soft port search: %w", err) } // Wide search if m.port == nil { - if err := m.SearchPort(false); err != nil { + if err := m.searchPort(false); err != nil { return fmt.Errorf("not soft port search: %w", err) } } if m.port == nil { - return errors.New("no port is detected") + return fmt.Errorf("no port is detected") } // Connect @@ -108,188 +93,37 @@ func (m *modem) Init() error { return nil } -func (m *modem) checkPort(portName string) error { - // Check device for existance - if _, err := os.Stat(portName); err != nil { - return fmt.Errorf("device does not exist") - } - - // Check serial connection - // Connect - m.port = at.New(portName, m.baudrate) - if err := m.port.Connect(); err != nil { - return fmt.Errorf("connect: %w", err) - } - defer m.port.Disconnect() // Do not bother about errors... - - // Ping - m.logger.Println("Ping...") - - if err := m.Ping(); err != nil { - return fmt.Errorf("ping error: %w", err) - } - - // Check model - m.logger.Println("Check model...") - - resp, err := m.port.Send("AT+CGMM") - if err != nil { - return fmt.Errorf("get model: %w", err) - } - if !resp.Check() { - return fmt.Errorf("error response: %s", resp) - } - model := strings.Split(resp.String(), "\n")[0] - if err != nil { - return fmt.Errorf("get model: %w", err) - } - rightModel := "SIMCOM_SIM7600E-H" - // m.logger.Printf("[% x]\n [% x]", []byte("SIMCOM_SIM7600E-H"), []byte(model)) - if model[:len(rightModel)] != rightModel { - return fmt.Errorf("invalid modem model: %s", model) - } - m.logger.Println("Model right") - return nil -} - -func (m *modem) SearchPort(isSoft bool) error { - // Get ports - ports := []string{"ttyUSB1", "ttyUSB2", "ttyUSB3"} - if !isSoft { - ps, err := GetTtyDevices() - if err != nil { - fmt.Errorf("get serial devices: %w", err) - } - ports = ps - } - - // Check ports -SearchLoop: - for _, p := range ports { - m.logger.Printf("Checking port %s ...\n", p) - - if err := m.checkPort("/dev/" + p); err != nil { - m.logger.Printf("Check failed: %s\n", err.Error()) - continue SearchLoop - } - - m.logger.Print("Found modem on port: ", p) - m.port = at.New("/dev/"+p, m.baudrate) - m.isAvailable = true - return nil - } - return nil -} - func (m *modem) Connect() error { - if m.port == nil { + if !m.IsAvailable() { return fmt.Errorf("port is not defined") } return m.port.Connect() } -func (m *modem) Ping() error { - resp, err := m.port.Send("AT") - if err != nil { - return err +func (m *modem) Disconnect() error { + if !m.IsAvailable() { + return fmt.Errorf("port is not defined") } - if !resp.Check() { - return fmt.Errorf("connection lost") - } - return nil + return m.port.Disconnect() } -func (m *modem) SwitchToGpsMode() error { - m.logger.Println("Enabling GPS mode...") - // Reset intput - if err := m.port.GetSerialPort().ResetInputBuffer(); err != nil { - return fmt.Errorf("reset input buffer: %w", err) - } - - // Check gps mode status - resp, err := m.port.Send("AT+CGPS?") - if err != nil { - return fmt.Errorf("make at ask: %w", err) - } - if !resp.Check() { - return fmt.Errorf("error response") - } - ans := strings.Replace(strings.Split(strings.Split(resp.RmFront("+CGPS:").String(), "\n")[0], ",")[0], " ", "", -1) - if ans == "1" { - m.logger.Println("GPS already enabled") - return nil - } - m.logger.Println(ans) - - // Modem is not in GPS mode - resp, err = m.port.Send("AT+CGPS=1") - if err != nil { - return fmt.Errorf("try to switch to gps: %w", err) - } - if !resp.Check() { - return fmt.Errorf("switch tp GPS failed") - } - m.logger.Println("GPS mode enabled") - return nil +func (m *modem) IsAvailable() bool { + return m.port != nil } -func deg2rad(deg float64) float64 { - return deg * (math.Pi / 180) -} - -func (m *modem) CalculateSpeed(newLatitude, newLongitude float64) { - m.logger.Println("Calculate speed") - earthRad := 6371.0 // TODO ? - dLat := deg2rad(math.Abs(newLatitude - m.gpsInfo.Latitude)) - dLon := deg2rad(math.Abs(newLongitude - m.gpsInfo.Longitude)) - a := math.Sin(dLat/2)*math.Sin(dLat/2) + - math.Cos(deg2rad(newLatitude))*math.Cos(deg2rad(m.gpsInfo.Latitude))*math.Sin(dLon/2)*math.Sin(dLon/2) - c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a)) - - m.gpsInfo.Speed = earthRad * c / (math.Abs(float64(time.Since(m.lastUpdateTime)))) -} - -func decodeGpsInfo(str string) (GpsInfo, error) { - var err error - newGpsInfo := GpsInfo{} - strs := strings.Split(strings.Split(strings.Replace(str, " ", "", -1), "\n")[0], ",") - - newGpsInfo.Latitude, err = strconv.ParseFloat(strs[0], 64) - if err != nil { - return GpsInfoNil, fmt.Errorf("parse latitude: %w", err) +func (m *modem) IsConnected() bool { + if m.IsAvailable() { + return m.port.IsConnected() } - newGpsInfo.Longitude, err = strconv.ParseFloat(strs[2], 64) - if err != nil { - return GpsInfoNil, fmt.Errorf("parse longitude: %w", err) - } - newGpsInfo.LatitudeIndicator = strs[1] - newGpsInfo.LatitudeIndicator = strs[3] - newGpsInfo.Date = strs[4] - newGpsInfo.Time = strs[5] - newGpsInfo.Altitude, err = strconv.ParseFloat(strs[6], 64) - if err != nil { - return GpsInfoNil, fmt.Errorf("parse altitude: %w", err) - } - newGpsInfo.Speed, err = strconv.ParseFloat(strs[7], 64) - if err != nil { - return GpsInfoNil, fmt.Errorf("parse speed: %w", err) - } - // Course sometimes may be null - if len(strs[8]) > 0 { - newGpsInfo.Course, err = strconv.ParseFloat(strs[8], 64) - if err != nil { - return GpsInfoNil, fmt.Errorf("parse course: %w", err) - } - } - return newGpsInfo, nil + return false } func (m *modem) Update() error { - m.logger.Println("Update") - if !m.isAvailable { + if !m.IsConnected() { m.logger.Println("No connection to module") return nil } + m.logger.Println("Update") // ans, err := m.port.Request(at.CmdQuestion, "CGPSINFO") // if err != nil { // return fmt.Errorf("check GPS info mode: %w", err) @@ -316,16 +150,29 @@ func (m *modem) Update() error { } m.logger.Println("Decoding data...") - newGpsInfo, err := decodeGpsInfo(strings.Split(strings.Replace(resp.RmFront("+CGPSINFO:").String(), "\r", "", -1), "\n")[0]) - if err != nil { + if err := m.gpsInfo.decode(strings.Split(strings.Replace(resp.RmFront("+CGPSINFO:").String(), "\r", "", -1), "\n")[0]); err != nil { m.logger.Println("Gps info decode error:", err.Error()) return nil } - m.gpsInfo = newGpsInfo m.logger.Println("Decoded successfully") return nil } +func (m *modem) TestGPS() error { + m.logger.Println("Testing GPS") + + if err := m.switchToGpsMode(); err != nil { + return fmt.Errorf("switch to GPS: %w", err) + } + + if err := m.Update(); err != nil { + return fmt.Errorf("update: %w", err) + } + + m.logger.Println("Current coords:", m.GetShortInfo()) + return nil +} + type ModemInfo struct { Port string `json:"Port"` GpsInfo @@ -368,18 +215,126 @@ func (m *modem) SaveGPS(path string) error { return nil } -func (m *modem) TestGPS() error { - m.logger.Println("Testing GPS") +func (m *modem) checkPort(portName string) (outErr error) { + defer func() { + if outErr != nil { // Clear port if there is error + m.port = nil + } + }() - if err := m.SwitchToGpsMode(); err != nil { - return fmt.Errorf("switch to GPS: %w", err) + // Check device for existance + if _, err := os.Stat(portName); err != nil { + return fmt.Errorf("device does not exist") } - if err := m.Update(); err != nil { - return fmt.Errorf("update: %w", err) + // Check serial connection + // Connect + m.port = at.New(m.logger, portName, m.baudrate) + if err := m.port.Connect(); err != nil { + return fmt.Errorf("connect: %w", err) + } + defer m.port.Disconnect() // Do not bother about errors... + + // Ping + m.logger.Println("Ping...") + + if err := m.ping(); err != nil { + return fmt.Errorf("ping error: %w", err) } - m.logger.Println("Current coords:", m.GetShortInfo()) + // Check model + m.logger.Println("Check model...") + + resp, err := m.port.Send("AT+CGMM") + if err != nil { + return fmt.Errorf("get model: %w", err) + } + if !resp.Check() { + return fmt.Errorf("error response: %s", resp) + } + model := strings.Split(resp.String(), "\n")[0] + if err != nil { + return fmt.Errorf("get model: %w", err) + } + rightModel := "SIMCOM_SIM7600E-H" + // m.logger.Printf("[% x]\n [% x]", []byte("SIMCOM_SIM7600E-H"), []byte(model)) + if model[:len(rightModel)] != rightModel { + return fmt.Errorf("invalid modem model: %s", model) + } + m.logger.Println("Model right") + return nil +} + +func (m *modem) searchPort(isSoft bool) error { + // Get ports + ports := []string{"ttyUSB1", "ttyUSB2", "ttyUSB3"} + if !isSoft { + ps, err := GetTtyDevices() + if err != nil { + fmt.Errorf("get serial devices: %w", err) + } + ports = ps + } + + // Check ports +SearchLoop: + for _, p := range ports { + m.logger.Printf("Checking port %s ...\n", p) + + if err := m.checkPort("/dev/" + p); err != nil { + m.logger.Printf("Check failed: %s\n", err.Error()) + continue SearchLoop + } + + m.logger.Print("Found modem on port: ", p) + m.port = at.New(m.logger, "/dev/"+p, m.baudrate) + return nil + } + return nil +} + +func (m *modem) ping() error { + resp, err := m.port.Send("AT") + if err != nil { + return err + } + if !resp.Check() { + return fmt.Errorf("connection lost") + } + return nil +} + +func (m *modem) switchToGpsMode() error { + m.logger.Println("Enabling GPS mode...") + // Reset intput + if err := m.port.GetSerialPort().ResetInputBuffer(); err != nil { + return fmt.Errorf("reset input buffer: %w", err) + } + + // Check gps mode status + resp, err := m.port.Send("AT+CGPS?") + if err != nil { + return fmt.Errorf("make at ask: %w", err) + } + if !resp.Check() { + return fmt.Errorf("error response") + } + ans := strings.Replace(strings.Split(strings.Split(resp.RmFront("+CGPS:").String(), "\n")[0], ",")[0], " ", "", -1) + if ans == "1" { + m.logger.Println("GPS already enabled") + return nil + } + m.logger.Println(ans) + + // Modem is not in GPS mode + resp, err = m.port.Send("AT+CGPS=1") + if err != nil { + return fmt.Errorf("try to switch to gps: %w", err) + } + if !resp.Check() { + return fmt.Errorf("switch tp GPS failed") + } + m.logger.Println("GPS mode enabled") return nil } diff --git a/main.go b/main.go index e68f10b..a90f3c6 100644 --- a/main.go +++ b/main.go @@ -2,6 +2,7 @@ package main import ( "log" + "os" "github.com/CGSG-2021-AE4/modem-test/api/modem" ) @@ -15,7 +16,7 @@ func main() { } func mainE() error { - m := modem.New() + m := modem.New(log.New(os.Stdout, "modem:", log.LstdFlags)) if err := m.Init(); err != nil { return err }