add B3_AP support

This commit is contained in:
Matthew GONG
2020-07-28 11:39:26 +08:00
parent 96750b2419
commit d059b0d584
8 changed files with 823 additions and 0 deletions
+85
View File
@@ -0,0 +1,85 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//#include "param_mgr.h"
#include "adc_devs.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
int is_analog_input_init = 0;
void analog_input_init()
{
if (!is_analog_input_init)
{
#ifdef HAL_IMPL
//TODO
is_analog_input_init = 1;
#endif
}
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_ai_Start_wrapper(void)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
analog_input_init();
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_ai_Outputs_wrapper(const uint16_T *start_idx,
uint16_T *AI,
int32_T *ErrorCode,
const int_T y_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
if (is_analog_input_init)
{
#ifdef HAL_IMPL
/*
* input *AI* from ADC channels from *start_idx* to *start_idx*+y_width-1.
*
* ErrorCode - return zero on success, otherwise nonzero on failures.
*/
// TODO set analog inputs
for (int_T i=0; i<y_width; ++i)
{
if (start_idx[0] + i <ADC_CH_NUM)
{
AI[i] = adc_values_v[start_idx[0]+i]*1000;
}
}
ErrorCode[0] = 0;
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+84
View File
@@ -0,0 +1,84 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//#include "param_mgr.h"
#include "gpio_devs.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
int is_discrete_input_init = 0;
void discrete_input_init()
{
if (!is_discrete_input_init)
{
#ifdef HAL_IMPL
//TODO
is_discrete_input_init = 1;
#endif
}
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_di_Start_wrapper(void)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
discrete_input_init();
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_di_Outputs_wrapper(const uint16_T *start_idx,
boolean_T *DI,
int32_T *ErrorCode,
const int_T y_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
if (is_discrete_input_init)
{
#ifdef HAL_IMPL
/*
* input *DI* from DI devices from *start_idx* to *start_idx*+y_width-1.
*
* ErrorCode - return zero on success, otherwise nonzero on failures.
*/
for (int_T i=0;i<y_width;++i)
{
if (start_idx[0]+i < GPIO_READPIN_SIZE)
{
DI[i] = GPIO_ReadPin(start_idx[0]+i);
}
}
ErrorCode[0] = 0;
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+85
View File
@@ -0,0 +1,85 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//#include "param_mgr.h"
#include "gpio_devs.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define y_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
int is_discrete_output_init = 0;
void discrete_output_init()
{
if (!is_discrete_output_init)
{
#ifdef HAL_IMPL
//TODO
is_discrete_output_init = 1;
#endif
}
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_do_Start_wrapper(void)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
discrete_output_init();
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_do_Outputs_wrapper(const boolean_T *DO,
const uint16_T *start_idx,
int32_T *ErrorCode,
const int_T u_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
if (is_discrete_output_init)
{
#ifdef HAL_IMPL
/*
* output *DO* to digital output devices from *start_idx* to *start_idx*+u_width-1.
*
* ErrorCode - return zero on success, otherwise nonzero on failures.
*/
for (int_T i=0;i<u_width;++i)
{
if (start_idx[0]+i < GPIO_WRITEPIN_SIZE)
{
GPIO_WritePin(start_idx[0]+i, DO[i]);
}
}
ErrorCode[0] = 0;
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+86
View File
@@ -0,0 +1,86 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//#include "param_mgr.h"
#include <stdint.h>
extern uint16_t RPM[9];
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
int is_pwm_in_init = 0;
void pwm_in_init()
{
if (!is_pwm_in_init)
{
#ifdef HAL_IMPL
//TODO
is_pwm_in_init = 1;
#endif
}
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_pwm_in_Start_wrapper(void)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
pwm_in_init();
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_pwm_in_Outputs_wrapper(const uint16_T *start_idx,
uint16_T *pwm,
int32_T *ErrorCode,
const int_T y_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
if (is_pwm_in_init)
{
#ifdef HAL_IMPL
int_T i;
/*
* input *pwm* from pwm devices from *start_idx* to *start_idx*+y_width-1.
*
* ErrorCode - return zero on success, otherwise nonzero on failures.
*/
for (i = 0; i< y_width; ++i)
{
if (start_idx[0]+i < 9)
{
pwm[i] = RPM[start_idx[0]+i];
}
}
ErrorCode[0] = 0;
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+105
View File
@@ -0,0 +1,105 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
#include "param_mgr.h"
#include "tim_devs.h"
#endif
#include <string.h>
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define y_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
void get_pwm0peroid(param_value_ptr_t p)
{
//TODO
p.I[0] = 400u;
}
void set_pwm0peroid(param_value_ptr_t p)
{
uint32_t peroid_req;
peroid_req = p.I[0];
//TODO
}
#endif
int is_pwm_out_init = 0;
void pwm_out_init()
{
if (!is_pwm_out_init)
{
#ifdef HAL_IMPL
//TODO
param_prop p;
strncpy(p.name, "pwm_peroid_g0", 16);
p.val_ptr.s = NULL;
p.getter_ptr = &get_pwm0peroid;
p.setter_ptr = &set_pwm0peroid;
p.typ = PARAM_TYPE_UINT32;
//param_mgr_regist(&p);
is_pwm_out_init = 1;
#endif
}
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_pwm_out_Start_wrapper(void)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
pwm_out_init();
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_pwm_out_Outputs_wrapper(const uint16_T *pwm,
const uint16_T *start_idx,
int32_T *ErrorCode,
const int_T u_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
if (is_pwm_out_init)
{
#ifdef HAL_IMPL
/*
* output *pwm* to pwm devices from *start_idx* to *start_idx*+u_width-1.
*
* ErrorCode - return zero on success, otherwise nonzero on failures.
*/
for (int i = 0; i< u_width; ++i)
{
set_pwm(start_idx[0]+i, pwm[i]);
}
ErrorCode[0] = 0;
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+92
View File
@@ -0,0 +1,92 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
#include "hal_api.h"
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//#include "param_mgr.h"
#include "sbus_conn.h"
#include <string.h>
#endif
#include <stdint.h>
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define y_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
int is_sbus_in_init[1];
void sbus_input_init(uint8_t id)
{
switch (id)
{
case 0:
if (!is_sbus_in_init[0])
{
#ifdef HAL_IMPL
//TODO config channels, update rate, etc. and bind properties
is_sbus_in_init[0] = 1;
#endif
}
}
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_sbus_in_Start_wrapper(const uint8_T *id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
sbus_input_init(id[0]);
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_sbus_in_Outputs_wrapper(HAL_sbus_in_t *sbus,
int32_T *ErrorCode,
const uint8_T *id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
ErrorCode[0] = -1;
/*
* input *AI* from ADC channels from *start_idx* to *start_idx*+y_width-1.
*
* ErrorCode - return zero on success, otherwise nonzero on failures.
*/
switch (id[0])
{
case 0u:
if (is_sbus_in_init[0])
{
#ifdef HAL_IMPL
sbus->valid = sbus_in.valid;
sbus->seq = sbus_in.seq;
sbus->id = id[0];
memcpy(sbus->channels, sbus_in.ch, sizeof(sbus->channels));
ErrorCode[0] = 0;
#endif
}
break;
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+134
View File
@@ -0,0 +1,134 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
#include "uart_fifos.h"
extern uint8_t queue_in[8][256];
extern uint8_t queue_hdr[8];
extern uint8_t queue_tal[8];
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_uart_in_Start_wrapper(const uint8_T *uart_id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//initialize_uart_device(uart_id[0]);
#endif
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_uart_in_Outputs_wrapper(uint8_T *Data,
uint16_T *Data_len,
int32_T *ErrorCode,
const uint8_T *uart_id, const int_T p_width0,
const int_T y_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
/* **********************************************************
* collect all data from this uart and push to Data buffer;
*
* parameter:
* uart_id - uart FIFO device to read
*
* outputs:
* Data - read all FIFO data and write to Data array
* Data_len[0] - the number of written data
* ErrorCode[0] - return zero for success, others for failail.
*
* constant:
* y_width - maximum length to write to Data array
************************************************************/
Data_len[0] = 0u;
ErrorCode[0] = -1;
switch (uart_id[0])
{
case 0:
Data_len[0] = UART_RX_FIFO_read(&uart1_RS422_1_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 1:
Data_len[0] = UART_RX_FIFO_read(&uart2_RS422_2_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 2:
Data_len[0] = UART_RX_FIFO_read(&uart3_IMU_NAV_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 3:
Data_len[0] = UART_RX_FIFO_read(&uart4_TTL_1_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 4:
Data_len[0] = UART_RX_FIFO_read(&uart5_STORE_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 5:
Data_len[0] = UART_RX_FIFO_read(&uart6_RS485_1_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 6:
Data_len[0] = UART_RX_FIFO_read(&uart7_RS232_1_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 7:
Data_len[0] = UART_RX_FIFO_read(&uart8_RS232_2_rx, Data, y_width);
ErrorCode[0] = 0;
break;
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
case 15:
{
int id = uart_id[0]-8;
Data_len[0] = 0;
while (queue_tal[id] != queue_hdr[id])
{
Data[Data_len[0]++] = queue_in[id][queue_tal[id]++];
}
}
ErrorCode[0] = 0;
break;
}
#else
/* dummy function for generate mex file for sim*/
Data_len[0] = 0u;
ErrorCode[0] = 0;
#endif
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
+152
View File
@@ -0,0 +1,152 @@
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
#include "uart_fifos.h"
#include "spi_devs.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define y_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Start function
*
*/
void hal_uart_out_Start_wrapper(const uint8_T *uart_id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
//initialize_uart_device(uart_id[0]);
#endif
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output function
*
*/
void hal_uart_out_Outputs_wrapper(const uint8_T *buff,
const uint16_T *len,
const uint8_T *sep,
int32_T *ErrorCode,
const uint8_T *uart_id, const int_T p_width0,
const int_T u_width)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
/* ***********************************************
* push all data to this uart output queue;
*
* inputs:
* buff - data array to be sent
* len[0] - available length in the data array
* sep[0] - it means the data array updated when sep[0] changed,
* uart_id[0] - uart FIFO device which is required to send data
*
* output:
* ErrorCode[0] - return zero for success, others for failail.
*
* constant:
* u_width - the maximum size of buff, do not read buff out of range.
*************************************************/
ErrorCode[0] = -1;
static uint8_T seq_last[16];
switch (uart_id[0])
{
case 0:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart1_RS422_1_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 1:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart2_RS422_2_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 2:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart3_DBG_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 3:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart4_TTL_1_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 4:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart5_STORE_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 5:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart6_RS485_1_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 6:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart7_RS232_1_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 7:
if (sep[0] != seq_last[uart_id[0]] && UART_TX_FIFO_write(&uart8_RS232_2_tx, (uint8_t *)buff, len[0]))
{
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
case 15:
if (sep[0] != seq_last[uart_id[0]])
{
SPI_Bridge_pack(&bdg, uart_id[0]-7, (uint8_t *)buff, len[0], 1);
seq_last[uart_id[0]] = sep[0];
ErrorCode[0] = 0;
}
break;
}
#else
/* dummy function for generate mex file for sim*/
ErrorCode[0] = 0;
#endif
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}