The Accel Application

 

The Accel Application

Let's put this newfound knowledge to work by writing an application that scrolls. Accel draws a window that resembles Microsoft Excel. (See Figure 2-13.) The spreadsheet depicted in the window is 26 columns wide and 99 rows high—much too large to be displayed all at once. However, scroll bars allow the user to view all parts of the spreadsheet. In addition to providing a hands-on look at the principles discussed in the preceding sections, Accel demonstrates another way that a program can scale its output. Rather than use a non-MM_TEXT mapping mode, it uses CDC::GetDeviceCaps to query the display device for the number of pixels per inch displayed horizontally and vertically. Then it draws each spreadsheet cell so that it's 1 inch wide and ¼ inch tall using raw pixel counts.

Click to view at full size.

Figure 2-13. The Accel window.

Figure 2-14. The Accel application.

Accel.h

#define LINESIZE 8

class CMyApp : public CWinApp
{
public:
    virtual BOOL InitInstance ();
};

class CMainWindow : public CFrameWnd
{
protected:
    int m_nCellWidth;   // Cell width in pixels
    int m_nCellHeight;  // Cell height in pixels
    int m_nRibbonWidth; // Ribbon width in pixels
    int m_nViewWidth;   // Workspace width in pixels
    int m_nViewHeight;  // Workspace height in pixels
    int m_nHScrollPos;  // Horizontal scroll position
    int m_nVScrollPos;  // Vertical scroll position
    int m_nHPageSize;   // Horizontal page size
    int m_nVPageSize;   // Vertical page size

public:
    CMainWindow ();
protected:
    afx_msg void OnPaint ();
    afx_msg int OnCreate (LPCREATESTRUCT lpCreateStruct);
    afx_msg void OnSize (UINT nType, int cx, int cy);
    afx_msg void OnHScroll (UINT nCode, UINT nPos, 
        CScrollBar* pScrollBar);
    afx_msg void OnVScroll (UINT nCode, UINT nPos, 
        CScrollBar* pScrollBar);

    DECLARE_MESSAGE_MAP ()
};

Accel.cpp

#include <afxwin.h>
#include "Accel.h"

CMyApp myApp;

/////////////////////////////////////////////////////////////////////////
// CMyApp member functions

BOOL CMyApp::InitInstance ()
{
    m_pMainWnd = new CMainWindow;
    m_pMainWnd->ShowWindow (m_nCmdShow);
    m_pMainWnd->UpdateWindow ();
    return TRUE;
}

/////////////////////////////////////////////////////////////////////////
// CMainWindow message map and member functions

BEGIN_MESSAGE_MAP (CMainWindow, CFrameWnd)
    ON_WM_CREATE ()
    ON_WM_SIZE ()
    ON_WM_PAINT ()
    ON_WM_HSCROLL ()
    ON_WM_VSCROLL ()
END_MESSAGE_MAP ()

CMainWindow::CMainWindow ()
{
    Create (NULL, _T ("Accel"),
        WS_OVERLAPPEDWINDOW ¦ WS_HSCROLL ¦ WS_VSCROLL);
}


int CMainWindow::OnCreate (LPCREATESTRUCT lpCreateStruct)
{

    if (CFrameWnd::OnCreate (lpCreateStruct) == -1)
        return -1;

    CClientDC dc (this);
    m_nCellWidth = dc.GetDeviceCaps (LOGPIXELSX);
    m_nCellHeight = dc.GetDeviceCaps (LOGPIXELSY) / 4;
    m_nRibbonWidth = m_nCellWidth / 2;
    m_nViewWidth = (26 * m_nCellWidth) + m_nRibbonWidth;
    m_nViewHeight = m_nCellHeight * 100;
    return 0;
}

void CMainWindow::OnSize (UINT nType, int cx, int cy)
{
    CFrameWnd::OnSize (nType, cx, cy);

    //
    // Set the horizontal scrolling parameters.
    //
    int nHScrollMax = 0;
    m_nHScrollPos = m_nHPageSize = 0;

    if (cx < m_nViewWidth) {
        nHScrollMax = m_nViewWidth - 1;
        m_nHPageSize = cx;
        m_nHScrollPos = min (m_nHScrollPos, m_nViewWidth -
            m_nHPageSize - 1);
    }


    SCROLLINFO si;
    si.fMask = SIF_PAGE ¦ SIF_RANGE ¦ SIF_POS;
    si.nMin = 0;
    si.nMax = nHScrollMax;
    si.nPos = m_nHScrollPos;
    si.nPage = m_nHPageSize;

    SetScrollInfo (SB_HORZ, &si, TRUE);

    //
    // Set the vertical scrolling parameters.
    //
    int nVScrollMax = 0;
    m_nVScrollPos = m_nVPageSize = 0;

    if (cy < m_nViewHeight) {
        nVScrollMax = m_nViewHeight - 1;
        m_nVPageSize = cy;
        m_nVScrollPos = min (m_nVScrollPos, m_nViewHeight -
            m_nVPageSize - 1);
    }

    si.fMask = SIF_PAGE ¦ SIF_RANGE ¦ SIF_POS;
    si.nMin = 0;
    si.nMax = nVScrollMax;
    si.nPos = m_nVScrollPos;
    si.nPage = m_nVPageSize;

    SetScrollInfo (SB_VERT, &si, TRUE);
}

void CMainWindow::OnPaint ()
{
    CPaintDC dc (this);

    //
    // Set the window origin to reflect the current scroll positions.
    //
    dc.SetWindowOrg (m_nHScrollPos, m_nVScrollPos);

    //
    // Draw the grid lines.
    //


    CPen pen (PS_SOLID, 0, RGB (192, 192, 192));
    CPen* pOldPen = dc.SelectObject (&pen);


    for (int i=0; i<99; i++) {
        int y = (i * m_nCellHeight) + m_nCellHeight;
        dc.MoveTo (0, y);
        dc.LineTo (m_nViewWidth, y);
    }

    for (int j=0; j<26; j++) {
        int x = (j * m_nCellWidth) + m_nRibbonWidth;
        dc.MoveTo (x, 0);
        dc.LineTo (x, m_nViewHeight);
    }

    dc.SelectObject (pOldPen);
    
    //
    // Draw the bodies of the rows and the column headers.
    //
    CBrush brush;
    brush.CreateStockObject (LTGRAY_BRUSH);

    CRect rcTop (0, 0, m_nViewWidth, m_nCellHeight);
    dc.FillRect (rcTop, &brush);
    CRect rcLeft (0, 0, m_nRibbonWidth, m_nViewHeight);
    dc.FillRect (rcLeft, &brush);

    dc.MoveTo (0, m_nCellHeight);
    dc.LineTo (m_nViewWidth, m_nCellHeight);
    dc.MoveTo (m_nRibbonWidth, 0);
    dc.LineTo (m_nRibbonWidth, m_nViewHeight);

    dc.SetBkMode (TRANSPARENT);

    //
    // Add numbers and button outlines to the row headers.
    //
    for (i=0; i<99; i++) {
        int y = (i * m_nCellHeight) + m_nCellHeight;
        dc.MoveTo (0, y);
        dc.LineTo (m_nRibbonWidth, y);

        CString string;
        string.Format (_T ("%d"), i + 1);

        CRect rect (0, y, m_nRibbonWidth, y + m_nCellHeight);
        dc.DrawText (string, &rect, DT_SINGLELINE ¦


            DT_CENTER ¦ DT_VCENTER);

        rect.top++;
        dc.Draw3dRect (rect, RGB (255, 255, 255),
            RGB (128, 128, 128));
    }

    //
    // Add letters and button outlines to the column headers.
    //

    for (j=0; j<26; j++) {
        int x = (j * m_nCellWidth) + m_nRibbonWidth;
        dc.MoveTo (x, 0);
        dc.LineTo (x, m_nCellHeight);

        CString string;
        string.Format (_T ("%c"), j + `A');

        CRect rect (x, 0, x + m_nCellWidth, m_nCellHeight);
        dc.DrawText (string, &rect, DT_SINGLELINE ¦
            DT_CENTER ¦ DT_VCENTER);

        rect.left++;
        dc.Draw3dRect (rect, RGB (255, 255, 255),
            RGB (128, 128, 128));
    }
}

void CMainWindow::OnHScroll (UINT nCode, UINT nPos, CScrollBar* pScrollBar)
{
    int nDelta;

    switch (nCode) {

    case SB_LINELEFT:
        nDelta = -LINESIZE;
        break;

    case SB_PAGELEFT:
        nDelta = -m_nHPageSize;
        break;

    case SB_THUMBTRACK:
        nDelta = (int) nPos - m_nHScrollPos;
        break;

    case SB_PAGERIGHT:
        nDelta = m_nHPageSize;
        break;

    case SB_LINERIGHT:
        nDelta = LINESIZE;
        break;
    default: // Ignore other scroll bar messages
        return;
    }

    int nScrollPos = m_nHScrollPos + nDelta;
    int nMaxPos = m_nViewWidth - m_nHPageSize;

    if (nScrollPos < 0)
        nDelta = -m_nHScrollPos;
    else if (nScrollPos > nMaxPos)
        nDelta = nMaxPos - m_nHScrollPos;

    if (nDelta != 0) {
        m_nHScrollPos += nDelta;
        SetScrollPos (SB_HORZ, m_nHScrollPos, TRUE);
        ScrollWindow (-nDelta, 0);
    }
}

void CMainWindow::OnVScroll (UINT nCode, UINT nPos, CScrollBar* pScrollBar)
{
    int nDelta;

    switch (nCode) {

    case SB_LINEUP:
        nDelta = -LINESIZE;
        break;

    case SB_PAGEUP:
        nDelta = -m_nVPageSize;
        break;

    case SB_THUMBTRACK:
        nDelta = (int) nPos - m_nVScrollPos;
        break;

    case SB_PAGEDOWN:
        nDelta = m_nVPageSize;
        break;

    case SB_LINEDOWN:
        nDelta = LINESIZE;
        break;

    default: // Ignore other scroll bar messages
        return;
    }

    int nScrollPos = m_nVScrollPos + nDelta;
    int nMaxPos = m_nViewHeight - m_nVPageSize;

    if (nScrollPos < 0)
        nDelta = -m_nVScrollPos;
    else if (nScrollPos > nMaxPos)
        nDelta = nMaxPos - m_nVScrollPos;

    if (nDelta != 0) {
        m_nVScrollPos += nDelta;
        SetScrollPos (SB_VERT, m_nVScrollPos, TRUE);
        ScrollWindow (0, -nDelta);
    }
}

GetDeviceCaps is called from CMainWindow's OnCreate handler, which is called upon receipt of a WM_CREATE message. WM_CREATE is the first message a window receives. It is sent just once, and it arrives very early in the window's lifetime—before the window is even visible on the screen. An ON_WM_CREATE entry in the window's message map connects WM_CREATE messages to the member function named OnCreate. OnCreate is the ideal place to initialize member variables whose values can only be determined at run time. It is prototyped as follows:

afx_msg int OnCreate (LPCREATESTRUCT lpCreateStruct)

lpCreateStruct is a pointer to a structure of type CREATESTRUCT, which contains useful information about a window such as its initial size and location on the screen. The value returned by OnCreate determines what Windows does next. If all goes as planned, OnCreate returns 0, signaling to Windows that the window was properly initialized. If OnCreate returns -1, Windows fails the attempt to create the window. A prototype OnCreate handler looks like this:

int CMainWindow::OnCreate (LPCREATESTRUCT lpCreateStruct)
{
    if (CFrameWnd::OnCreate (lpCreateStruct) == -1)
        return -1;

            
      
    return 0;
}

OnCreate should always call the base class's OnCreate handler to give the framework the opportunity to execute its own window-creation code. This is especially important when you write document/view applications, because it is a function called by CFrameWnd::OnCreate that creates the view that goes inside a frame window.

You'll find the code that does the scrolling in the window's OnHScroll and OnVScroll handlers. switch-case logic converts the notification code passed in nCode into a signed nDelta value that represents the number of pixels the window should be scrolled. Once nDelta is computed, the scroll position is adjusted by nDelta pixels and the window is scrolled with the statements

m_nVScrollPos += nDelta;
SetScrollPos (SB_VERT, m_nVScrollPos, TRUE);
ScrollWindow (0, -nDelta);

for the vertical scroll bar and

m_nHScrollPos += nDelta;
SetScrollPos (SB_HORZ, m_nHScrollPos, TRUE);
ScrollWindow (-nDelta, 0);

for the horizontal scroll bar.

How are the scroll positions stored in m_nHScrollPos and m_nVScrollPos factored into the program's output? When OnPaint is called to paint the part of the workspace that was exposed by the scrolling operation, it repositions the window origin with the statement

dc.SetWindowOrg (m_nHScrollPos, m_nVScrollPos);

Recall that CDC::SetWindowOrg tells Windows to map the logical point (x,y) to the device point (0,0), which, for a client-area device context, corresponds to the upper left corner of the window's client area. The statement above moves the origin of the coordinate system left m_nHScrollPos pixels and upward m_nVScrollPos pixels. If OnPaint tries to paint the pixel at (0,0), the coordinate pair is transparently transformed by the GDI into (_m_nHScrollPos,_m_nVScrollPos). If the scroll position is (0,100), the first 100 rows of pixels are clipped from the program's output and the real output—the output the user can see—begins with the 101st row. Repositioning the origin in this manner is a simple and effective way to move a scrollable window over a virtual display surface.

If you could enlarge the window enough to see the entire spreadsheet, you would see the scroll bars disappear. That's because CMainWindow::OnSize sets the scroll bar range to 0 if the window size equals or exceeds the size of the virtual workspace. The OnSize handler also updates the scrolling parameters whenever the window size changes so that the thumb size accurately reflects the relative proportions of the window and the virtual workspace.

And with that, all the pieces are in place. The user clicks a scroll bar or drags a scroll bar thumb; OnHScroll or OnVScroll receives the message and responds by updating the scroll position and scrolling the window; and OnPaint redraws the window, using SetWindowOrg to move the drawing origin an amount that equals the current scroll position. The program's entire workspace is now accessible, despite the physical limitations that the window size imposes on the output. And all for less than 100 additional lines of code. How could it be any easier?

Funny you should ask. Because that's exactly what MFC's CScrollView class is for: to make scrolling easier. CScrollView is an MFC class that encapsulates the behavior of a scrolling window. You tell CScrollView how large a landscape you wish to view, and it handles everything else. Among other things, CScrollView processes WM_VSCROLL and WM_HSCROLL messages for you, scrolls the window in response to scroll bar events, and updates the thumb size when the window size changes.

While it's perfectly possible to wire a CScrollView into an application like Accel, CScrollView was designed primarily for document/view applications. Chapter 10 examines CScrollView more closely and also introduces some of the other view classes that MFC provides.

MBEDTLS_CHECK_RETURN_CRITICAL static int ssl_prepare_record_content(mbedtls_ssl_context *ssl, mbedtls_record *rec) { int ret, done = 0; MBEDTLS_SSL_DEBUG_BUF(4, "input record from network", rec->buf, rec->buf_len); #if defined(MBEDTLS_SSL_HW_RECORD_ACCEL) if (mbedtls_ssl_hw_record_read != NULL) { MBEDTLS_SSL_DEBUG_MSG(2, ("going for mbedtls_ssl_hw_record_read()")); ret = mbedtls_ssl_hw_record_read(ssl); if (ret != 0 && ret != MBEDTLS_ERR_SSL_HW_ACCEL_FALLTHROUGH) { MBEDTLS_SSL_DEBUG_RET(1, "mbedtls_ssl_hw_record_read", ret); return MBEDTLS_ERR_SSL_HW_ACCEL_FAILED; } if (ret == 0) { done = 1; } } #endif /* MBEDTLS_SSL_HW_RECORD_ACCEL */ if (!done && ssl->transform_in != NULL) { unsigned char const old_msg_type = rec->type; if ((ret = mbedtls_ssl_decrypt_buf(ssl, ssl->transform_in, rec)) != 0) { MBEDTLS_SSL_DEBUG_RET(1, "ssl_decrypt_buf", ret); #if defined(MBEDTLS_SSL_DTLS_CONNECTION_ID) if (ret == MBEDTLS_ERR_SSL_UNEXPECTED_CID && ssl->conf->ignore_unexpected_cid == MBEDTLS_SSL_UNEXPECTED_CID_IGNORE) { MBEDTLS_SSL_DEBUG_MSG(3, ("ignoring unexpected CID")); ret = MBEDTLS_ERR_SSL_CONTINUE_PROCESSING; } #endif /* MBEDTLS_SSL_DTLS_CONNECTION_ID */ return ret; } if (old_msg_type != rec->type) { MBEDTLS_SSL_DEBUG_MSG(4, ("record type after decrypt (before %d): %d", old_msg_type, rec->type)); } MBEDTLS_SSL_DEBUG_BUF(4, "input payload after decrypt", rec->buf + rec->data_offset, rec->data_len); #if defined(MBEDTLS_SSL_DTLS_CONNECTION_ID) /* We have already checked the record content type * in ssl_parse_record_header(), failing or silently * dropping the record in the case of an unknown type. * * Since with the use of CIDs, the record content type * might change during decryption, re-check the record * content type, but treat a failure as fatal this time. */ if (ssl_check_record_type(rec->type)) { MBEDTLS_SSL_DEBUG_MSG(1, ("unknown record type")); return MBEDTLS_ERR_SSL_INVALID_RECORD; } #endif /* MBEDTLS_SSL_DTLS_CONNECTION_ID */ if (rec->data_len == 0) { #if defined(MBEDTLS_SSL_PROTO_TLS1_2) if (ssl->minor_ver == MBEDTLS_SSL_MINOR_VERSION_3 && rec->type != MBEDTLS_SSL_MSG_APPLICATION_DATA) { /* TLS v1.2 explicitly disallows zero-length messages which are not application data */ MBEDTLS_SSL_DEBUG_MSG(1, ("invalid zero-length message type: %d", ssl->in_msgtype)); return MBEDTLS_ERR_SSL_INVALID_RECORD; } #endif /* MBEDTLS_SSL_PROTO_TLS1_2 */ ssl->nb_zero++; /* * Three or more empty messages may be a DoS attack * (excessive CPU consumption). */ if (ssl->nb_zero > 3) { MBEDTLS_SSL_DEBUG_MSG(1, ("received four consecutive empty " "messages, possible DoS attack")); /* Treat the records as if they were not properly authenticated, * thereby failing the connection if we see more than allowed * by the configured bad MAC threshold. */ return MBEDTLS_ERR_SSL_INVALID_MAC; } } else { ssl->nb_zero = 0; } #if defined(MBEDTLS_SSL_PROTO_DTLS) if (ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM) { ; /* in_ctr read from peer, not maintained internally */ } else #endif { unsigned i; for (i = 8; i > mbedtls_ssl_ep_len(ssl); i--) { if (++ssl->in_ctr[i - 1] != 0) { break; } } /* The loop goes to its end iff the counter is wrapping */ if (i == mbedtls_ssl_ep_len(ssl)) { MBEDTLS_SSL_DEBUG_MSG(1, ("incoming message counter would wrap")); return MBEDTLS_ERR_SSL_COUNTER_WRAPPING; } } } #if defined(MBEDTLS_SSL_DTLS_ANTI_REPLAY) if (ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM) { mbedtls_ssl_dtls_replay_update(ssl); } #endif /* Check actual (decrypted) record content length against * configured maximum. */ if (rec->data_len > MBEDTLS_SSL_IN_CONTENT_LEN) { MBEDTLS_SSL_DEBUG_MSG(1, ("bad message length")); return MBEDTLS_ERR_SSL_INVALID_RECORD; } return 0; }
最新发布
12-26
/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" #include "i2c.h" #include "usart.h" #include "gpio.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "mpu6050.h" #include "stdio.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ MPU6050_t MPU6050; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_I2C1_Init(); MX_USART1_UART_Init(); /* USER CODE BEGIN 2 */ while(MPU6050_Init(&hi2c1)==1) { } /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { MPU6050_Read_All(&hi2c1, &MPU6050); printf("加速度 x:%.2f \t y:%.2f \t z:%.2f\n",MPU6050.Ax,MPU6050.Ay,MPU6050.Az); printf("陀螺仪 x:%.2f \t y:%.2f \t z:%.2f\n",MPU6050.Gx,MPU6050.Gy,MPU6050.Gz); printf("温度 %.2f\n",MPU6050.Temperature); MPU6050_Read_All(&hi2c1, &MPU6050); printf("X: %f, Y: %f\n", MPU6050.KalmanAngleX, MPU6050.KalmanAngleY); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ /* USER CODE END 4 */ /** * @brief This function is executed in case of error occurrence. * @retval None */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ __disable_irq(); while (1) { } /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */ /* * mpu6050.c * * Created on: Nov 13, 2019 * Author: Bulanov Konstantin * * Contact information * ------------------- * * e-mail : leech001@gmail.com */ /* * |--------------------------------------------------------------------------------- * | Copyright (C) Bulanov Konstantin,2021 * | * | This program is free software: you can redistribute it and/or modify * | it under the terms of the GNU General Public License as published by * | the Free Software Foundation, either version 3 of the License, or * | 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 General Public License for more details. * | * | You should have received a copy of the GNU General Public License * | along with this program. If not, see <http://www.gnu.org/licenses/>. * | * | Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter * |--------------------------------------------------------------------------------- */ #include <math.h> #include "mpu6050.h" #define RAD_TO_DEG 57.295779513082320876798154814105 #define WHO_AM_I_REG 0x75 #define PWR_MGMT_1_REG 0x6B #define SMPLRT_DIV_REG 0x19 #define ACCEL_CONFIG_REG 0x1C #define ACCEL_XOUT_H_REG 0x3B #define TEMP_OUT_H_REG 0x41 #define GYRO_CONFIG_REG 0x1B #define GYRO_XOUT_H_REG 0x43 // Setup MPU6050 #define MPU6050_ADDR 0xD0 const uint16_t i2c_timeout = 100; const double Accel_Z_corrector = 14418.0; uint32_t timer; Kalman_t KalmanX = { .Q_angle = 0.001f, .Q_bias = 0.003f, .R_measure = 0.03f}; Kalman_t KalmanY = { .Q_angle = 0.001f, .Q_bias = 0.003f, .R_measure = 0.03f, }; uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx) { uint8_t check; uint8_t Data; // check device ID WHO_AM_I HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout); if (check == 104) // 0x68 will be returned by the sensor if everything goes well { // power management register 0X6B we should write all 0's to wake the sensor up Data = 0; HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout); // Set DATA RATE of 1KHz by writing SMPLRT_DIV register Data = 0x07; HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout); // Set accelerometer configuration in ACCEL_CONFIG Register // XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> � 2g Data = 0x00; HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, i2c_timeout); // Set Gyroscopic configuration in GYRO_CONFIG Register // XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> � 250 �/s Data = 0x00; HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout); return 0; } return 1; } void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) { uint8_t Rec_Data[6]; // Read 6 BYTES of data starting from ACCEL_XOUT_H register HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout); DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]); DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]); DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]); /*** convert the RAW values into acceleration in 'g' we have to divide according to the Full scale value set in FS_SEL I have configured FS_SEL = 0. So I am dividing by 16384.0 for more details check ACCEL_CONFIG Register ****/ DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0; DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0; DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector; } void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) { uint8_t Rec_Data[6]; // Read 6 BYTES of data starting from GYRO_XOUT_H register HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout); DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]); DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]); DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]); /*** convert the RAW values into dps (�/s) we have to divide according to the Full scale value set in FS_SEL I have configured FS_SEL = 0. So I am dividing by 131.0 for more details check GYRO_CONFIG Register ****/ DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0; DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0; DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0; } void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) { uint8_t Rec_Data[2]; int16_t temp; // Read 2 BYTES of data starting from TEMP_OUT_H_REG register HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout); temp = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]); DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53); } void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) { uint8_t Rec_Data[14]; int16_t temp; // Read 14 BYTES of data starting from ACCEL_XOUT_H register HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout); DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]); DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]); DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]); temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]); DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]); DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]); DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]); DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0; DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0; DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector; DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53); DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0; DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0; DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0; // Kalman angle solve double dt = (double)(HAL_GetTick() - timer) / 1000; timer = HAL_GetTick(); double roll; double roll_sqrt = sqrt( DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW + DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW); if (roll_sqrt != 0.0) { roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG; } else { roll = 0.0; } double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG; if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90)) { KalmanY.angle = pitch; DataStruct->KalmanAngleY = pitch; } else { DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt); } if (fabs(DataStruct->KalmanAngleY) > 90) DataStruct->Gx = -DataStruct->Gx; DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt); } double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt) { double rate = newRate - Kalman->bias; Kalman->angle += dt * rate; Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle); Kalman->P[0][1] -= dt * Kalman->P[1][1]; Kalman->P[1][0] -= dt * Kalman->P[1][1]; Kalman->P[1][1] += Kalman->Q_bias * dt; double S = Kalman->P[0][0] + Kalman->R_measure; double K[2]; K[0] = Kalman->P[0][0] / S; K[1] = Kalman->P[1][0] / S; double y = newAngle - Kalman->angle; Kalman->angle += K[0] * y; Kalman->bias += K[1] * y; double P00_temp = Kalman->P[0][0]; double P01_temp = Kalman->P[0][1]; Kalman->P[0][0] -= K[0] * P00_temp; Kalman->P[0][1] -= K[0] * P01_temp; Kalman->P[1][0] -= K[1] * P00_temp; Kalman->P[1][1] -= K[1] * P01_temp; return Kalman->angle; }; /* * mpu6050.h * * Created on: Nov 13, 2019 * Author: Bulanov Konstantin */ #ifndef INC_GY521_H_ #define INC_GY521_H_ #endif /* INC_GY521_H_ */ #include <stdint.h> #include "i2c.h" // MPU6050 structure typedef struct { int16_t Accel_X_RAW; int16_t Accel_Y_RAW; int16_t Accel_Z_RAW; double Ax; double Ay; double Az; int16_t Gyro_X_RAW; int16_t Gyro_Y_RAW; int16_t Gyro_Z_RAW; double Gx; double Gy; double Gz; float Temperature; double KalmanAngleX; double KalmanAngleY; } MPU6050_t; // Kalman structure typedef struct { double Q_angle; double Q_bias; double R_measure; double angle; double bias; double P[2][2]; } Kalman_t; uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx); void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt); 为什么我的代码没办法输出
11-25
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值